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ABSTRACT 


/ 

/ 

A  sequential  extended  Kalman  filter  and  optimal 
smoothing  algorithm  was  developed  to  provide  real  time 
estimates  of  torpedo  position  and  depth  on  the  three 
dimensional  underwater  tracking  range  at  the  Naval  Torpedo 
Station,  Keyport,  Washington.  The  measurements  consisted  of 
acoustic  pulse  transit  times  from  the  torpedo  to  receiving 
array,  which  are  nonlinear  functions  of  the  positions  and 
the  depth  of  the  torpedo,  were  linearized  and  filter  gains 
and  filtered  estimates  of  states  calculated.  By  running  the 
smoothing  subroutine,  all  past  filtered  estimates  of  states 
and  error  covariance  were  smoothed.  The  program  was  tested, 
using  simulated  torpedo  trajectories  that  traversed  both 
single  and  multiple  arrays,  on  an  IBM-PC.  The  results  showed 
that  filter  performance  was  dependent  on  system  noise  and 
the  distance  to  the  hydrophone  array  from  the  torpedo  and 
the  smoothed  estimates  of  states  and  error  covariances  were 
better  than  or  equal  to  the  filtered  estimates. 
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I. 


INTRODUCTION 


The  Naval  Torpedo  Station  at  Keyport,  Washington 
currently  operates  two  three-dimensional  underwater  tracking 
ranges  utilizing  a  sonar  transmitter  installed  in  the 
torpedo  to  be  tracked.  The  transmitter  is  synchronized  with 
a  master  clock.  Timed  acoustic  pulses  are  received  by 
hydrophone  arrays  and  then  relayed  via  cable  to  a  computer 
at  the  observation  site.  The  computer  calculates  the 
positional  coordinates  o-f  the  torpedo  and  plots  its 
trajectory  through  the  water. 

The  measured  data,  which  consist  o-f  the  elapsed  time 
from  transmission  o-f  a*  pulse  until  its  receipt  at  the 
hydrophone  array,  is  corrupted  with  noise  due  to  combined 
offsets  of  environmental  -factors  and  measurement 
i nstruments. 

The  intention  is  to  implement  and  test  a  sequential 
extended  Kalman  -filter  and  smoothing  routine  which  processes 
the  transit  times  of  the  acoustic  pulses  and  generates  the 
filtered  and  smoothed  estimates  of  the  positions  of  tracked 
torpedo  at  a  particular  time.  The  design  takes  into  account 
the  elimination  of  the  storage  problem. 
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II. 


DESCRIPTION  OF  RANGE  TRACKING  GEDHETRY 

The  hydrophone  array,  consisting  of  four  independent 
elements,  defines  an  orthogonal  coordinate  system  in  which 
transit  time  measurements  are  made.  As  shown  in  Figure  2.1, 
four  hydrophones  X,  Y,  Z,  and  C  are  on  four  adjacent 
vertices  separated  -by  a  distance  d,  along  the  edge  of  the 
cube.  The  origin  of  the  array  coordinates  is  at  the  center 
of  the  cube  with  the  orthogonal  coordinates  parallel  to  its 
edge.  Positional  information  is  computed  from  the  transit 
times  of  a  periodic  synchronous  acoustic  signal  traveling 
from  the  torpedo  to  the  four  hydrophones  on  the  array.The 
.torpedoes  are  equipped  with  sonar  transmitters  which  are 
transmitting  an  acoustic  signal  in  every  1.31  seconds, 
within  a  range  accuracy  3  to  30  ft.  When  tracking  by 
multiple  arrays,  the  signal  from  the  closest  hydrophone 
array  is  defined  as  the  basis  for  the  time  measurements  and 
for  the  range  calculations.  A  more  detailed  description  of 


the  range  tracking  capability  is  described  in  CRef.  1,  23. 


Figure  2.1  Geometry  o-f  a  Tracking  Array 


III.  THEORY 


A.  EXTENDED  KALMAN  FILTER 

The  basic  idea  of  the  extended  Kalman  filter  is  to 

relinearize  about  each  estimate  X(k/k),  once  it  has  been 

computed.  As  soon  as  a  new  state  estimate  is  made,  a  new  and 
better  reference  state  trajectory  is  incorporated  into  the 
estimation  process.  CRef.  3,  4,  51 

For  the  three-dimensional  location  problem  three 

position  states  <X,  Y,  Z)  and  two  velocity  states  (V  ,  V  > 

x  y 

specify  target  motion.  The  discrete  linear  and  nonlinear 
observation  equations  are  given  by 

X  < k+1 )  *  £  .  X  (k )  +  p  .  W(k>  (3.1> 

and 

Z(k)  -  h(X(k>,  k)  +  V (k>  (3.2) 

where:  £  and  p  are  constant  matrices; 

h  is  a  nonlinear  function  of  the  state  X 

W(k)  is  the  plant  excitation  noise; 

V(k)  is  the  measurement  noise. 

In  these  equations  the  plant  noise  and  measurement  noise  are 
assumed  uncorrelated  (white)  with  zero  mean.  That  is, 

ECW(k)  .  WT  ( j)  1  =  Q‘  (k)  r,. 

~  k  j 
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where 


ECV(k) .V(j)3-R(k>  & 

""  K  J 

:  s  =  1 ,  k  =  j; 

=0,  k  -  j. 

In  order  to  apply  the  linear  -filter.  Equation  3.2  is 
expanded  in  a  Taylor  series  about  the  best  estimate  o-f  the 
state  at  that  time  and  only  the  first  order  terms  are  kept. 
Equation  3.2  gives 

Z  <  k )  »  H  < k )  .  X (k)  +  V  <k>  (3.3> 

where 

ah  | 

H  ( k )  =  ----  {  (3.4) 

!  X  <k)  =  X  < k/k  -  1) 

A 

X  < k/k  -  1)  is  a  predicted  value  of  the  state  at  time  k, 

given  the  measurements  until  time  k-1. 

A  state  error  vector  is  defined  by 

X  (k/k)  *  X (k/k)  -  X(k) , 

and  a  predicted  state  error  vector  is  defined  by 
X (k/k  -  1)  =  X (k/k  -  1)  -  X  ( k )  . 

The  covariance  of  state  error  matrix  is  defined  by 
P  ( k/k  >  =  EC  X ( k/k ) . X  T(k/k)l, 

the  predicted  covariance  of  state  error  matrix  is  given  by 
P  (k/k  -  1)  =  EC  X (k/k  -  1).X  T(k/k  -  1)3. 


J -f  -*■  r^i*jr»jr»  V"YTT*  jyj\ 


■ ■7“^TCT<^vrr,-rf 


B.  OPTIMAL  SMOOTHING 

Smoothing  is  a  non-real  time  data  processing  scheme  that 
uses  all  measurements  between  0  and  N  to  estimate  the  state 
of  a  system  at  certain  time  k,  where  0  k  ,<  N.  The  smoothed 
estimate  of  X(k)  based  on  all  measurements  between  0  and  N 


is  denoted  by  X(k/N).  The  smoothed  error  covariance  is 

denoted  by  P(k/N)  and  P(k/N)  P(k/k>  means  that  the 

smoothed  estimate  of  X(k)  is  at  least  as  good  as  the 

filtered  estimate  or  equal  to  its  filtered  estimate  for  all 
the  time,  except  the  terminal  time.  This  is  shown 
graphically  in  Figure  3.1.  As  portrayed  in  Figure  3.2,  there 
are  three  classes  of  particular  interest  because  of  their 
applicability  to  realistic  problems  CRef.  3,  4,  51.  One  is 
the  Rauch-Tung-Stri ebel  form,  which  was  chosen  in  our 


particular  problem  CRef.  6,  71. 

The  smoothed  state  estimate  and  the  smoothed  error 
covariance  matrix  are  given  by 

A  .A  A  A 

X  <  k/N)  =  X  <  k/k )  +  A ( k ) C  X ( k+1 /N>  -  X(k  +  l/k)l  (3.11) 


where 


X  ( k+1 /k )  »  $  X  <  k/k  ) 


P ( k/N) =P ( k/k ) +A  <k ) CP < k+1 /N> -P < k  +  1 /k ) 1A ( k ) 


A ( k )  =  P (k/k)  $T  P_1 <k+l/k> 


(3. 12) 


( 3 . 13) 


for  k  .<  N . 


IV.  PROBLEM  DEFINITION 


A.  OBSERVATION  AND  PLANT  STATE  EQUATIONS 

In  the  torpedo  tracking  problem,  the  non-linear 
observation  equations  are  the  -four  independent  transit  times 
from  the  tracked  torpedo  to  the  hydrophones,  T^ ,  T ^  ,  T^  and 

T  .  Thus  the  non-linqar  measurement  matrix  is  defined  by 

Z  (k)  »  CT  <k)  T  <k)  T  <k>  T  <k>  1T  *  V(k)  (4.1) 

-  c  x  y  2  - 

where 

T  (k)=--rC  <X  <k) +d/2)2  +<Y(k)+d/2)2  +(Z(k)+d/2)2  '  2 

c  vel 

T  <k)=--rC(X(k)-d/2)a+<Y(k)+d/2)J+(Z(k)+d/2)i!  l1 /2 
x  vel 

T  (k)=--rC(X(k)+d/2)2+(Y(k)-d/2)2  +  (Z(k)+d/2)2  l1 /2 
y  vel 

T  (k)=— ~C<X<k)+d/2)2+<Y(k)+d/2)2+(Z(k)-d/2)2  l*'2 

z  vel 

Since  the  transit  times  are  readily  available  and 
non-linear  functions  of  position,  these  equations  can  be 


linearized  and  Kalman  filter  theory  applied  using  the 
extended  Kalman  filter.  This  procedure  produces  a  real  time 


filtering  on  the  transit  times  T  ,  T  ,  T  and  T  ,  without 

c  x  y  z 

the  necessity  of  converting  these  times  to  positions. 

Equation  3.4  can  be  used  to  give  the  linearized 
observation  matrix.  UJhen  the  derivatives  are  taken  and 

evaluated  at  the  predicted  state  values  X(k/k-l)  =  X'  <k>  the 
result  is 

|  X^__( k_)__+_d/2  £jlO_+_d/2  tO_+_d/2  ! 

!  deni  deni  deni  ! 

!  X^jtk2_-_d/2  ZLlhl-1-2'2  j 

1  !  den2  den2  den2  ! 

Xl  ii<i_±_d/2  Y^_<k2_-_d/2  Z^__(  kj__+_d/2  j 

den3  deno  den 3  ! 

Xl_<k2._+_d/2  Y*  (k)  +  d/2  ZM  k2._-_d/2  i 

den4  den4  den4 

where: 

denl=»C  <X‘  <k)+d/2)2  + <Y*  (k) +d/2) 2  +  ( Z‘  ( k )  +d/2) 2  31  /2 

den2*C (X*  (k) -d/2)2  + <Y'  < k > +d/2> 2  + < Z' ( k ) +d/2) 2  l1 /2 

den3=E <X*  <k) +d/2)2  + ( Y*  < k ) -d/2) 2  +( Z *  ( k ) +d/2) 2  ]l  /2 

den4=C  (X*  (k) -t-d/2)2  -*- <  Y‘  (k ) +d/2) 2  +  ( Z’  < k )  -d/2>  2  l1  72 


H(k)« - 

vel 


*  > 


The  measurement  noises,  V(k) 's,  are  assumed  to  be 
zero-mean  and  independent  with  a  covariance  matrix 


R  <k> 


The  plant  state  equations  are 


j  X ( k+l )  | 

l  1 

|  X  <k) 

1 

+  T  V  (k) 

X 

+  gi  i 

S  v  <k+i>  i 

1  X  1 

j  V  <k> 

1  X 

*  92 

1 

J 

1 

« 

{  Y ( k+ 1 )  | 

I  > 

=  !  Y  (  k  ) 

+  T  V  Ck) 

y 

-  g,  j 

J  t 

(4.2) 

j  V  <k  +  l)  } 

j  V  <k> 

•  y 

+  <34 

1 

I 

* 

1 

j  Z  < k+l )  i 

l  1 

i  Z  <k> 

1 

*  g5 

t 

1 

» 

1 

where  X ( k )  ,  Y  <  k ) 

and  Z  ( k ) 

are  the  position 

coordinates  o-f 

the  torpedo  at  time  t<k) ,  V  <k)  and  V  (k>  are  the  X  and  V 

’  x  y 

components  o-f  the  velocity. 
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where  the  states  -are  evaluated  at  the  current  state 

A 

estimates  X<k/k>.  Substituting  these  expressions  in  the  Q 


>  -» 
>y«.  * ' 

•I-,*,*/ 

Si 

t 

v-S 

S'V 


matrix  results  in 


i <11,2 
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*r 


T3 
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T  «x. 
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A  more  detailed  derivation  of  the  excitation  covariance 
matrix  is  given  in  CRef.  83. 

The  excitation  matrix  serves  not  only  to  take  into 
account  the  possibility  of  maneuvering,  but  of  model 
inaccuracies  as  well.  Q  also  used  to  prevent  the  gains  of 
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the  filter  from  approaching  zero  as  more  and  more  data  is 


processed,  by  insuring  some  uncertainty  in  the  predicted 
state  values  CRef.  3,  4,  51. 

In  the  state  form,  the  plant  state  equation  is 

X ( k  +  1 )  =  1  X  < k )  +  r  W ( k )  (4.4) 

where: 


1 

T 

0 

0 

0  ! 

T2  /2 

0 

0  i 

0 

1 

0 

0 

0  i 

1 

T 

0 

o! 

0 

0 

1 

T 

0  1 

and 

1 

r  ■ 

0 

T2/2 

oi 

0 

0 

0 

1 

0  i 

0 

T 

oi 

0 

0 

0 

0 

i  i 

| 

0 

c> 

oi 

B.  DEFINITION  OF  MULTIPLE  ARRAY  TRACKING 

The  coordinate  system  is  defined  as  shown  in  Figure  4.1. 
These  72  positions,  an  XYZ  position  for  each  of  4 
hydrophones  in  6  arrays,  are  placed  into  a  6  x  12  matrix 
HYDRO  and  referenced  throughout  the  program.The  torpedo 
position  is  referenced  to  a  central  level  rectangular 
coordinate  system.  The  non-linear  observation  equations 
become 

Z  ( k )  =  IT  (k)  T  (k)  T  (k)  T  (k)]T  +  V(k)  (4.5) 

-  c  x  y  z  - 

T  ( k  )  =-— t-C  <  X ( k) -X . _ ) 2  +  ( Y  (  k  )  —  Y .  _ ) 2  +  (  Z  (k )  —  Z  _ )  2  l1  /2 


where 


•  V*  ,/v  v*  . ' 


\  ■%  ' %'  -.  ■v  % 


T  ( k )  =-— —  C  <  X  <  k ) -X .  „  ) 2  +  <Y(k)  —  Y.  v)2+(Z(k)-Z.  ,. ) 2  l*  '  2 
x  vel  1 X  1 X  i X 

T  (k)  =--TC  (X  <k)-X  .  w)  2  +  <Y  (k) -Y  .  w)  2  +  (Z  <k)-Z  .  2  31/2 

y  vel  lY  lY  lY 

T  <k)=-“C  <X<k)-X.  ,)2 +<Y(k>-Y.  ,)2 +<Z<k)-Z  ,)2  31  /2 

z  vel  i  z  i  z  u 

and  the  subscripted  variables  X,  Y,  and  Z  are  the 
coordinates  of  a  particu*  r  array  being  used. 

The  decision  parameter  used  to  determine  the  switching 
from  array  to  array  is  a  straight  handoff.  If  the  predicted 

x  position,  Xk+l/k  ’  is  greater  than  3,000  feet  from  the 

array  in  use,  then  index  is  incremented  and  the  next  row  of 
HYDRO  is  implememted.  This  placed  into  the  program  the  X,  Y, 
and  Z  positions  of  the  hydrophones  in  the  next  array.  The 
handoff  can  easily  be  utilized  in  real  range  operations,  as 
the  transit  times  from  adjacent  arrays  are  present  at  the 
computer  for  a  particular  time  slot. 
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a  )  Coordinate  System  -for  Multiple  Array  Tracking 


c 

HYDRO 

X  HYDRO  Y  .HYDRO 

Z  HYDRO 

X 

y  z 

x  y  z  x  y  z 

x  y  z 

36000 

1 

» 

6000 ! 0 

. . 7  ■  ■“  .  *  ■  ■  7  ■  . . r 

1  It  1  1 

36030 ! 6000 ! 0 ! 36000 ! 6030 1 0 

i  ! 

36000 ! 6000 ! 30 

30000 

6000 ! 0 

30030 ! 6000 ! 0 ( 30000 i 6030 1 0 

30000  1 6000 1 30 

24000 

6000  1 0 

24030 ! 6000 ! 0 1 24000 i 6030  j 0 

24000 ! 6000 i 30 

18000 

6000 ( 0 

1 8030  f  6000 ! 0  f 1 8000 ( 6030 i 0 

1 8000 ( 6000 ! 30 

12000 

6000 I 0 

1 2030 ( 6000 (Ofl 2000 ! 6030 ! 0 

12000(6000(30 

6000 

6000  ( 0 

6030 i 6000 1 0 !  6000 1 6030 ! 0 

6000 ( 6000 ( 30 

b  )  Hydrophone  Array  Location  Matrix 
Figure  4.1  Geometry  of  Multiple  Array  Tracking 
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c. 


SEQUENTIAL  EXTENDED  KALMAN  FILTER 


In  the  sequential  approach,  after  modifying  the  basic 

Kalman  filter  equations,  calculations  are  performed  on  each 

of  the  four  independent  transit  times  in  the  following 

order:  T  ,  T  ,  T  and  T  far  each  1.31  second  time  slot, 

c  x  y  z 

Since  the  four  transit  times  are  independent  and  processed 
sequentially,  the  covariance  of  error  matrix  and  the  state 
vector  are  updated  four  times  during  each  time  slot.  Thus 
more  accurate  estimates  of  the  filter  states  are  achived. 
Modification  of  the  filter  equations  for  the  sequential 
approach  circumvented  the  matrix  inversion  in  the  gain 
equation.  An  invalid  transit  time  measurement  will  result  in 
the  filter  ignoring  the  update  information  for  that 
particular  measurement  only. 

The  estimate  of  the  states  X(k/k),  based  on  one  transi t 


time  measurement  are  used  as  the  prediction  X(k/k-l)  for  the 

cal cul ati onson  the  next  measurements.  Thus  for  the  first 

time  measurement  T  only  the  first  row  of  the  linearized  H 

c 

matrix  is  calculated  and  then  the  first  gain  column 
corresponding  to  the  first  time  measurement  T^_  is  calculated 


by 


i  col 


H. 


1  row 


P  (k/k-1 ) 
P  <  k /k— 1 ) 


ht 

i  row 


ht 

i  row 


(4.6) 


1 1 


where  i  *  1  to  4  corresponding  to  the  tour  measured  transit 
t i mes . 

An  estimate  of  the  particular  observation  time  is 
calculated  by  using  Equation  3.9  evaluated  at  the  predicted 

-•X 

state  X(k/k-l).  The  difference  between  observed  transit 

times  and  the  estimated  transit  times  forms  the  residual 
which  is  used  in  the  estimate  equation 

X  .  *  X ( k/k— 1 )  +  G.  ,  CResi dual  3  (4.7) 

-  l  -  icol 


This  equation  gives  an  estimate  of  the  states  based  on  one 
of  the  four  time  measurements. 

The  covariance  of  error  is  calculated  based  on  one 
measurement  by 


P. 

l 


Cl  -  G 


icol 


H  3 

irow 


i-1 


(4.8) 


where:  I  is  identity  matrix; 

P^_j  is  the  covariance  matrix  calculated  from  the 

previous  transit  time  measurement  or  if  i  =  1, 
the  predicted  error  covariance  P(k/k-l). 

Editing  erroneous  time  measurement  is  achieved  by 
implementing  a  three  sigma  gate  using  the  covariance  of  the 


measurent  noise  (R)  and  the  covariance  of  the  estimation 


error  P(k/k).The  gate  then  is  written  for  each  time 
measurement  i  =  1  to  4: 

gate  =*  3*  <C <P .  maximum) / <4860. >8 3  +  R.  .  /2  (4.9) 

J  J  11 

where  j  ~  1,  3,  5.  The  gate  expands  or  decreases  depending 
on  the  confidence  level  of  the  position  estimate  and  the 
transit  time.  If  the  difference  between  the  actual  transit 
time  received  and  predicted  transit  time  to  a  particular 
hydrophone  exceeds  the  gate,  the  measurement  is  considered 
unacceptable  and  the  filter  gain  is  set  to  zero  causing  the 
filter  to  ignore  the  data  and  take  the  prediction  of  the 

states  as  the  estimate  X(k/k)  *  X<k/k-l). 

Bounding  the  residual  bias  error  is  achieved  by  making 
comparison  between  the  average  of  the  absolute  value  of  the 
time  residuals  and  the  preset  threshold.  If  the  average  of 
the  time  residuals  exceeds  the  preset  threshold,  Q  is 
calculated  and  added  to  the  last  updated  covariance  of  error 
matrix  P.  Then  filter  reiterates  the  gain,  covariance,  and 
state  estimate  equations  for  the  same  time  slot.  This 
procedure  continues  until  the  average  of  the  time  residuals 
falls  below  the  preset  threshold  at  which  time  an  acceptable 
state  vector  estimate  has  been  obtained  for  the  time  slot. 


D.  OPTIMAL  SMOOTHING  ALGORITHM 


The  smoothing  solution  starts  with  the  -filtered  estimate 
at  the  last  point  and  calculates  backward  point  by  point 
determining  the  smoothed  estimate  as  a  linear  combination  o-f 
the  -filtered  estimate  at  that  paint  arid  the  smoothed 
estimate  at  the  previous  point  CRe-f.  61. 

It  can  be  seen  -from  the  error  covariances  that  the 
-filter  has  reached -a  steady-state  condition  by  the  end  o-f 
the  -forward  sweep.  As  an  example,  let  us  enter  the  backward 

sweep  at  the  end  point  where  k  =»  20.  Here  we  have  X  (20/20) 

and  P(20/20).  Since  the  filter  solution  at  this  point  is 
conditioned  on  all  the  measurement  data,  it  is  also  the 
smoothed  estimate  at  k  »  N  =  20.  We  are  now  ready  to  compute 
the  smoothed  estimate  one  step  back  at  k  *  19.  From 
Equations  3.11,  3.12  and  3.13  we  have 

X  < 19/20) =  X(19/19)+  A  < 19) CX (20/20)  -  X(20/19>3 
stored  stored 


X (20/ 19)  a  £  X ( 19/ 19 ) 
stored 


=  P ( 19/ 19)  £T  P-1 (20/19) 
stored  stored 


A(  19) 


P  (  19/20)  =  P  <  19/  19)  +  A  ( 1 9 )  CPC  20 /  20 )  -  P(20/19)3  A  (19) 

stored  stored  stored 


and  to  compute  the  smoothed  estimate  two  step  back  at  k  * 


X(18/20)*  X(18/18)-»-  A  <  18)  C  X  (  19/20)  -  X(19/18): 
stored 


X ( 19/18)  ■  1  X  < 18/ 18) 
stored 


A  < 18)  *  P  < 18/18)  P_i  (19/18) 


stored 


stored 


P ( 18/20)  »  P ( 18/18)  +  A< 18) CP( 19/20)  -  P(19/18):  A  (18) 
stared  stored 


This  procedure  continues  until  the  time  k  reaches  to  1. 


V? 

w. 


V .  SIMULATION  RESULTS 

A.  MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 

The  true  trajectory  of  the  torpedo  is  a  straight  line 
with  a  50  ft/sec  velocity  toward  the  origin  of  hydrophone 
array  parallel  to  X-axis,  drawing  two  tangent  circles  with 
10  deg/sec  turn  rate,  in  the  horizantal  X-Y  plane  through  a 
multiple  array. 

In  the  first  part  o-f  this  run,  the  initial  position  of 
the  torpedo  is  38000  ft  in  X,  7000  ft  in  Y,  and  300  ft  in  Z. 
Figures  5.1  and  5.2  depict  the  filtered  and  smoothed 
estimate  of  the  trajectory,  with  zero  initial  velocity 
errors  and  25  ft  initial  position  errors  in  X  and  Y.  The 
errors  in  the  filtered  and  the  smoothed  estimate  of 
positions  in  X,  Y  and  Z  are  drawn  in  Figures  5.3,  5.4,  5.5, 
5.6,  5.7  and  5.8.  For  the  Kalman  filter,  errors  ranged 
between  -1.2  and  2.6  ft  in  X,  -5.9  and  1.9  ft  in  Y,  0.1  and 
2.5  ft  in  Z.  After  smoothing,  the  errors  occured  in  smaller 
range,  which  is,  between  -1.4  and  2.4  ft  in  X,  -5.0  and  1.9 
ft  in  Y,  0.1  and  0.7  ft  in  Z.  The  diagonal  terms  of  the 
filtered  and  smoothed  error  covariance  matrices  are  shown 
pictorially  in  Figures  5.9,  5.10,  5.11,  5.12,  5.13,  5.14, 
5.  15,  5. 16,  5. 17,  5.  18. 
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In  the  second  part  of  this  run,  the  initial  position  of 


the  torpedo  is  35000  ft  in  X,  7000  ft  in  Y,  and  300  ft  in  Z. 
The  filtered  and  smoothed  estimate  of  the  trajectory  are 
drawn  in  Figures  5.19  and  5.20.  Taking  this  different 
initial  geometry  made  the  errors  in  the  position  of  the 
torpedo  to  take  place  in  bigger  values  during  first  time 
slot  of  the  filtering  and  last  time  slot  of  the  smoothing. 
As  seen  in  Figures  5.21,  5.22,  5.23,  5.24,  5.25  and  5.26, 
errors  ranged  between  -16.3  and  1.9  ft  in  X,  -15.1  and  4.6 
ft  in  Y,  —5.0  and  1.0  ft  in  Z  for  the  Kalman  filter  and  for 
the  smoothing  this  error  range  is  between  -12.6  and  1.3  ft 
in  X,  -12.3  and  4.8  ft  in  Y,  -1.9  and  1.0  ft  in  Z.  The 
diagonal  terms  of  the  filtered  and  smoothed  error  covariance 
matrices  displayed  slightly  different  magnitude,  as  seen  in 
Figures  5.27,  5.28,  5.29,  5.30,  5.31,  5.32,  5.33,  5.34,  5.35 
and  u. o 6 . 

B.  MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUN 

In  this  run,  the  true  trajectory  of  the  torpedo  is  a 
straight  line  with  a  50  ft/sec  velocity  toward  the  origin  of 
hydrophone  array  parallel  to  X-axis  in  the  horizantal  X-Y 
plane  through  a  multiple  array. 

With  the  initial  position  of  the  torpedo  is  38000  ft  in 
X,  7000  ft  in  Y,  and  300  ft  in  Z.  The  filtered  and  smoothed 


estimate  of  the  trajectory 


with  zero  initial  velocity 


errors  and  25  -ft  initial  position  errors  in  X  and  Y,  are 
depicted  in  Figures  5.37  and  5.33.  Figures  5.39,  5.40,  5.41, 
5.42,  5.43  and  5.44  give  the  errors  in  the  filtered  and  the 
smoothed  estimate  of  positions  in  X,  Y  and  Z.  For  the  Kalman 
filter,  errors  ranged  between  -1.6  and  2.6  ft  in  X,  -5.9  and 
4.7  ft  in  Y,  —0.2  and  2.5  ft  in  Z.  After  smoothing,  the 
errors  occured  in  smaller  range,  which  is,  between  -1.1  and 
2.4  ft  in  X,  -5.0  and  1.7  ft  in  Y,  -0.2  and  0.6  ft  in  Z.  The 
diagonal  terms  of  the  filtered  and  smoothed  error  covariance 
matrices  are  shown  pictorially  in  Figures  5.45  through  5.54. 

C.  SINGLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 

The  previous  tests  described  the  filter  and  smoothing 
performance  for  both  straight  and  maneuvering  runs  through 
multiple  array.  Using  the  same  basic  torpedo  trajectories  as 
in  multiple  array,  similar  tests  are  performed  for 
maneuvering  run  through  single  array. During  the  single  array 
tracking,  the  initial  position  of  the  torpedo  is  7500  ft  in 
X,  1300  ft  in  Y  and  0  ft  in  Z,  which  gives  different  initial 
geometry.  The  filtered  and  smoothed  estimates  of  the 
trajectory  and  the  correspondi ng  position  errors  in  X,  Y  and 
Z  are  pictorially  given  in  Figures  5.55  through  5.62.  For 
the  Kalman  filter,  errors  ranged  between  -1.6  and  3.3  ft  in 
X,  -19.1  and  3.9  ft  in  Y,  -0.3  and  1.6  ft  in  Z.  After 
smoothing,  the  errors  occured  in  smaller  range,  which  is. 


between  -1.1  and  3.1  -ft  in  X,  -17.9  and  5.0  -ft  in  Y,  -0.1 
and  1.6  -ft  in  Z.  The  diagonal  terms  of  the  -filtered  and 
smoothed  error  covariance  matrices  are  shown  pictorially  in 
Figures  5.63  through  5.72. 

D.  SINGLE  ARRAY  STRAIGHT  RUN 

The  purpose  of  this  last  series  of  tests  is  to 
functionally  demonstrate  the  performance  of  the  filter  and 
smoothing  during  a  straight  run  through  single  array  using 
the  same  initial  torpedo  position  as  in  single  array 
adaptive  maneuvering  run.  The  filtered  and  smoothed 
estimates  of  the  trajectory  and  the  corresponding  position 
errors  in  X,  Y  and  Z  are  pictorially  given  in  Figures  5.73 
through  5.80.  For  the  Kalman  filter,  errors  ranged  between 
-1.6  and  3.3  ft  in  X,  -19.1  and  8.9  ft  in  Y,  -0.3  and  0.8  ft 
in  Z.  After  smoothing,  the  errors  occured  in  smaller  range, 
which  is,  between  -0.6  and  3.1  ft  in  X,  -17.9  and  3.5  ft  in 
Y,  -0.2  and  0.7  ft  in  Z.  The  diagonal  terms  of  the  filtered 
and  smoothed  error  covariance  matrices  are  shown  pictorially 
in  Figures  5.80  through  5.90. 
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V I .  CONCLUSIONS 

The  sequential  extended  Kalman  -filter  and  smoothing 
routine  su-f -f  i  ci  entl  y  generated  the  -filtered  and  smoothed 
estimates  o-f  the  states,  which  speci-fy  the  motion  o-f  the 
torpedo.  Errors  generated  by  running  the  routine  on  the 
IBM-PC  are  comparable  to  those  given  in  the  previous  search, 
which  was  done  on  a  large  IBM  computer  [Re-f.  13. 

In  the  smoothing  problem,  computing  the  predicted 

A 

estimates  o-f  the  states,  X<k+l/k>,  -from  the  estimates  o-f  the 

.a.  .•*. 

states,  X  (k/k )  ,  eliminates  the  storage  problem  -for  X(k+l/k). 

In  future  studies,  an  algorithm  for  computing  P(k/k)  from 
P (k+l/k+1 )  and  hence  eliminating  the  storage  problem  for 
P<k/k),  should  be  invesigated. 

Examining  the  errors  and  their  covariances,  it  is 
evident  that  the  uncertainty  in  position  exist  only  in  the 
Y  direction  for  the  case  where  the  torpedo  is  moving  along 
the  X  axis.  The  results  of  the  straight  run  analyses  shew 
that  the  propagation  of  the  filtered  error  covariance  is 
dependent  on  the  path  of  the  torpedo  with  respect  to 
hydrophone  array.  Upon  observing  the  error  propagation  it  is 
apparent  that  the  position  errors  exhibit  approx i matel y 
equal  oscillations  about  zero  indicating  that  the 
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_-v  ’  .  V  %  1 
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measurement  noise  is  the  dominant  error  source  driving  the 
■filter. 

The  smoothed  estimates  o-f  the  states  are  at  least  as 
good  as  or  better  than  the  •filtered  estimates.  The  -filter 
performance  was  dependent  on  system  noise  and  the  distance 
■from  the  torpedo  to  the  hydrophone  array.  Errors  get  bigger 
as  the  torpedo  approaches  the  tracking  limit  of  the 
hydrophone  array. 

Additional  work  should  be  done  using  trajectories 
generated  from  actual  torpedo  runs  on  the  Dabob  test  range. 
The  rotation  and  reduction  of  the  error  ellipsoids  should 
be  also  included  in  future  studies. 

The  filter  should  be  of  use  in  range  safety  in  warning 
for  passible  collisions.  Also  it  may  prove  invaluable  in 
torpedo  recovery  when  there  is  a  malfunction  and  the  torpedo 
is  sometimes  buried  in  many  feet  of  mude. 
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MULTIPLE  ARRAT  SORPTIVE  MANEUVERING  RUN 
FILTERED  ESTIMATE  OF  TRAJECTORY  WITH  NOISE  i 


3350  3430  3510  3590  3670  3750  3830| 

XCK/KD  (FT)  *  L  0 1 


Figure  3.1  Filtered  Estimate  of  Trajectory  of  the  Torpedo 
During  a  Maneuvering  Run  through  Multiple  Array 


multiple  array  adaptive  maneuvering  run 

SMOOTHED  ESTIMATE  OF  TRAJECTORY  WITH  NOISE 
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Figure  5.2  Smoothed  Estimate  of  Trajectory  of  the  Torpedo 
During  a  Maneuvering  Run  through  Multiple  Array 
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Figure  5.11  Variance  o-f  Filtered  Velocity  Error  in  X  of  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


Figure  5.12  Variance  o-f  Smoothed  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


Figure  5-15  Variance  o-f  Filtered  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


Figure  5.16  Variance  o-f  Smoothed  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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MULTIPLE  ARRAY  AOAPTIVE  MANEUVERING  RUN 
ERROR  IN  FILTERED  ESTIMATE  WITH  NOISE 
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Figure  5.21  Error  in  Filtered  Estimate  o-f  Position  in  X  at 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


Figure  5.22  Error  in  Smoothed  Estimate  o-f  Position  in  X  o-f 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


Figure  5.23  Error  in  Filtered  Estimate  of  Position  in  Y  of 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


Figure  5.24  Error  in  Smoothed  Estimate  o-f  Position  in  Y  of 
the  Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 
FILTERED  ERROR  COVARIANCE  P IK/K)  WITH  NOISE 
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Figure  5.29  Variance  of  Filtered  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 
SMOOTHED  ERROR  COVARIANCE  P IK/N)  WITH  NOISE 
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Figure  5.30  Variance  o-f  Smoothed  Velocity  Error  in  X  of  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


MULTIPLE  ARRAY  AOAPTIVE  MANEUVERING  RUN 
FILTERED  ERROR  COVARIANCE  P  (K/K)  WITH  NOISE 


Figure  5.31  Variance  o-f  Filtered  Position  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


Figure  5.32  Variance  o-f  Smoothed  Position  Error  i  n  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 


MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 
FILTERED  ERROR  COVARIANCE  P  IK/K)  WITH  NOISE 
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Figure  5-33  Variance  of  Filtered  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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SMOOTHED  ERROR  COVARIANCE  P  (K/N)  WITH  NOISE 
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Figure  5.34  Variance  o-f  Smoothed  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Maneuvering  Run  through  Multiple  Array 
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MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUN 
F^TEREO  ESTIMATE  OF  TRAJECTORY  WITH  NOISE 


5-37  Filtered  Estimate  o-f  Trajectory  of  the  Torpedo 
During  a  Straight  Run  through  Multiple  Array 


MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUN 
SMOOTHED  ESTIMATE  OF  TRAJECTORY  WITH  NOISE 


Figure  5.3S  Smoothed  Estimate  of  Trajectory  of  the  Torpedo 
During  a  Straight  Run  through  Multiple  Array 


Figure  5.41  Error  in  Filtered  Estimate  of  Position  in  Y  of 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 


Figure  5.42  Error  in  Smoothed  Estimate  o-f  Position  in  Y  of 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.43  Error  in  Filtered  Estimate  of  Position  in  Z  of 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 


Figure  5.44  Error  in  Smoothed  Estimate  o-f  Position  in  Z  of 
the  Torpedo  During  a  Straight  Run  through  Multiple  Array 


MULTIPLE-  RRRflT  AOflPTIVE  STRAIGHT  RUN 
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Figure  5.45  Variance  o-f  Filtered  Position  Error  in  X  of  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 


Figure  5.46  Variance  of  Smoothed  Position  Error  in  X  of  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 


Figure  5.47  Variance  o-f  Filtered  Velocity  Error  in  X  o-f  th 
Torpedo  During  a  Straight  Run  through  Multiple  Array 


Figure  5.4S  Variance  o-f  Smoothed  Velocity  Error  in  X  o-f  th 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUN  | 
FILTERED  ERROR  COVARIANCE  P  IK/K)  WITH  NOISE  j 
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Figure  5.49  Variance  of  Filtered  Position  Error  in  Y  of  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 


MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUN 
SMOOTHED  ERROR  COVARIANCE  P 1K/N)  WITH  NOISE 


O 

o 


Figure  5.50  Variance  o-f  Smoothed  Position  Error  in  Y  of  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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FILTERED  ERROR  COVARIANCE  P  IK/K)  WITH  NOISE 


MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUM 
FILTEREO  ERROR  COVRRiflNCE  P  IK/K)  WITH  NOISE 


Figure  5.53  Variance  o-f  Filtered 
Torpedo  During  a  Straight  Run 
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Figure  5.54  Variance  o-f  Smoothed  Position  Error  in  Z  o-f  the 
Torpedo  During  a  Straight  Run  through  Multiple  Array 
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Figure  5.55  Filtered  Estimate  of  Trajectory  o-f  the  Torpedo 
During  a  Maneuvering  Run  through  Single  Array 


SINGLE  ARRAY  AOAPTIVE  MANEUVERING  RUN 
FILTERED  ERROR  COVARIANCE  P  (K/K)  WITH  NOISE 


Figure  5.63  Variance  o-f  Filtered  Position  Error  in  X  of  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.64  Variance  of  Smoothed  Position  Error  in  X  of  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.65  Variance  of  Filtered  Velocity  Error  in  X  of  the 
Torpedo  During  a  Maneuvering  Run  through  Sinlge  Array 


SINGLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 
SMOOTHED  ERROR  COVARIANCE  P  (K/N)  WITH  NOISE 


Figure  5.66  Variance  of  Smoothed  Velocity  Error  in  X  of  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 


Figure  5.67  Variance  o-f  Filtered  Position  Error  in  Y  o-f  th 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 


Figure  5.68  Variance  o-f  Smoothed  Position  Error  in  Y  o-f  th 
Torpedo  During  a  Maneuvering  Run  .through  Single  Array 


SINGLE  ARRAY  AOAPTIVE  MANEUVERING  RUN 
FILTERED  ERROR  COVARIANCE  P  (K/K)  WITH  NOISE 


Figure  3.69  Variance  of  Filtered  Velocity  Error  in  Y  of  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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SMOOTHED  ERROR  COVARIANCE  P  (K/N)  WITH  NOISE 
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Figure  5.70  Variance  of  Smoothed  Velocity  Error  in  Y  of  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 


Figure  5.71  Variance  of  Filtered  Position  Error  in  Z  of  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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Figure  5.72  Variance  of  Smoothed  Position  Error  in  Z  of  the 
Torpedo  During  a  Maneuvering  Run  through  Single  Array 
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gure  5. 75  Error  in  Filtered  Estimate  o-f  Position  in  X  of 
the  Torpedo  During  a  Straight  Run  through  Single  Array 


gure  5.76  Error  in  Smoothed  Estimate  o-f  Position  i  n  X  of 
the  Torpedo  During  a  Straight  Run  through  Single  Array 


CCC 


SINGLE  ARRAY  AOAPT.IVE  STRAIGHT  RUN 
FILTERED  ERROR  COVARIANCE  P  IK/K)  WITH  NOISE 


Figure  5.31 
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Variance  a-f  Filtered  Position  Error  in  X  o-f 
During  a  Straight  Run  through  Single  Array 
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Figure  5.82  Variance  o*f  Smoothed  Position  Error  in  X  o*f  the 
Torpedo  During  a  Straight  Run  through  Single  Array 


SINGLE  ARRAY  ADAPTIVE  STRAIGHT  RUN 
FILTERED  ERROR  COVARIANCE  P  IK/K)  WITH  NOISE 


Figure  5.83  Variance  o-f  Filtered  Velocity  Error  in  X  o-f  the 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.84  Variance  o-f  Smoothed  Velocity  Error  1  n  X  o-f  the 
Torpedo  During  a  Straight  Run  through  Single  Array 
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SINGLE  ARRAY  ADAPT  I VE  STRAIGHT  RUN 
FILTEREO  ERROR  COVARIANCE  P  (K/K)  WITH  NOISE 
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Figure  5.87  Variance  o-f  Filtered  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Straight  Run  through  Single  Array 
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Figure  5.S8  Variance  o-f  Smoothed  Velocity  Error  in  Y  o-f  the 
Torpedo  During  a  Straight  Run  through  Single  Array 


APPENDIX  A 
PROGRAM  DESCRIPTION 


A.  GENERAL 

The  sequential  extended  Kalman  filter  and  Smoothing 
routine  is  described  in  detail  by  CRef.  ID.  Implementation 
is  done  by  using  F0RTRAN77  compilers  on  IBM-PC.  [Ref.  10, 
11,  12,  13D. 

B.  RUNNING  THE  PROGRAM  ON  THE  IBM-PC 

These  directions  apply  for  the  IBM— PC  computer  or  other 
computers (compati bl es)  with  two  f loopy  disk-drives,  640k 
memory,  col  or /graphi c  board,  math  coprocessor  and  paralel 
dot  matrix  printer  or  printer/plotter .  The  software  utilized 
during  the  simulation  studies  ares 

1.  Operating  System  DOS  2.10  with  required  files  to 
create  virtual  disk  and  full  screen  editor 
utilities. 

2.  IBM  Professional  FORTRAN  Compiler  1.00. 

3.  Microsoft  F0RTRAN77  3.20. 

4.  Plotworks  PL0T88.LIB. 

3.  Source  files. 

Getting  the  sequential  extended  Kalman  filter  and 
smoothing  routine  started  is  essentially  a  five  step 
process:  start  your  computer;  edit  the  source  file  and  make 

required  changes  and  then  compile;  run  the  executable  file 


and  get  the  data  to  be  available  -for  platting  routine;  edit 
the  source  file  of  plotting  routine  and  make  the  necessary 
changes  for  plotting  titles  and  then  compile;  run  platting 
routine.  Start  the  computer  up  with  an  operating  system  and 
get  the  program  running  simply  by  typing  "RUN",  which  is 
given  in  Appendix  E,  at  promt  "A>". 
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APPENDIX  B 

SEQUENTIAL  EXTENDED  KALMAN  FILTER  AND  OPTIMAL  SMOOTHING 

PROGRAM  LISTING 


PROGRAM  THESIS 


REAL*8  XKKM1 (5) ,PKKMl <5, 5) , PHI (5,5) , GAMMA (5,3) , GATE 
REAL  *8  GAMMAT  (3,5)  ,C0YW(3,3)  ,C0W(4,4)  ,QTEMP(5,3)  ,P 
REAL*8  TRUX ( 121 ) ,TRUV<121> ,TRUZ(121) ,ZI (4) ,HR0W(5) 
REAL *8  ZHAT,GDEN0M,GDTEMP,PDUM(3,5>  ,PI  (5,5)  ,WHTN,A14 
REALMS  ZD IFF (4) ,ZIC(4) ,XI (5) ,XKK(5) ,PKK<5,5) ,ZDIFAV 
REAL *8  DATR (17) , WINIT , PHIPKK (5 , 5) ,PKTEMP(5,5) ,SIGACC 
REAL*8  SIGDI V , SIGCC , XKERR ( 121 > ,YKERR(121) ,XP6(5,121) 
REAL*8  HYDRO (6, 12) , XB (4) , YB (4) , ZB < 4) ,XSERR(121> ,TD(3> 
REAL *8  SIGCCC,SIGAAC,SIGDDI , XP (5, 121 > ,SMTH(121> ,GI (5) 
REAL *8  P5(121,5,5) ,SS1 (121,5,5) ,Pi (121,5,5) ,Q(5,5) 
REAL*8  YSERR ( 121 ) ,ZS£RR(121) ,GNUM(5> ,PHIT(5,5) ,XP1 (5) 
REAL*8  ZD IFTO , CH (5,5) , TEMPI (5,5) ,XNNM1 (5) ,TEMP2(5) 
REAL *8  AK (5,5) ,AKT(5,5) ,TEMP3(5> ,TEMP4(5,5) ,XKKS(5) 
REAL *8  PNNM1 (5,5) ,TEMP5(5,5) ,TEMP6(5,5) ,PKKS(5,5) 

REAL *8  SS2 (5) ,P2(5,5) ,SS3(5,5> ,SS3R(5,5) ,SIG 
REAL *8  ZKERR ( 121 ) , X lkERR , X2KERR , Y1KERR , Y2KERR , Zl KERR 
REAL *8  Z2KERR , X 1 SERR , X2SERR , Y 1 SERR , Y2SERR , Z 1 SERR 
REAL *8  Z2SERR 

COORDINATES  OF  HYDROPHONE  ARRAY,  FOR  MULTIPLE  ARRAY 

DATA  HYDR0/36000. ,30000. ,24000. , 18000. , 12000. ,6000. 

*  , 6*6000 . , 6*0 . O , 36030 . , 30030 . , 24030 . , 1 8030 . ,12030. 

+  , 6030 . , 6*6000 . , 6*0 . O , 36000 . , 30000 . , 24000 . , 1 8000 . 

*  ,12000. ,6000. ,6*6030. ,6*0.0,36000. ,30000. ,24000. 

*  ,  18000.  ,  12000.  ,7*6000.  ,6*30.  / 

DATA  PKKM1/ 1000.0, 5*0.0, 1000.0,5*0.0, 1000.0,5*0.0 
+  ,1000.0,5*0.0,1000.0/ 

DATA  PHI/1 . ,4*0. ,1.31,1. ,5*0. , 1 . , 4*0. ,1.31,1. ,5*0. 

-*■  ,1./ 
DATA  GAMMA/O . 858,1.31,5*0.0,0.858,1 . 31,5*0.0, 1.31/ 
DATA  COVW/ 1.0, 3*0.0, 1.0, 3*0.0, 1 . 0/ , WINIT/O. 49/ 

DATA  COW/  1 .  OD-8 , 4*0.  0,1.  OD-8 , 4*0. 0, 1 . 0D-8 , 4*0.0 

*  ,  1 . OD— 8/ 
DATA  FOR  MULTIPLE  ARRAY  TRACKING 

DATA  DATR/38000. ,7000. ,300. ,-50. ,0. ,3*0. ,3*0. 
*,4.712389,. 1745329, . 1,8.1 ,600. ,800. / 

DATA  XKKM 1 /3797S . 0 , -50 . 0 , 6975 .0,0.0, 300. 0/ 

SECOND  DATA  FOR  MULTIPLE  ARRAY  TRACKING 

DATA  DATR/35000. ,7000. ,300. ,-50. ,0. ,3*0. ,3*0. 
*,4.712389,. 1745329, . 1,8.1 ,600. ,800. / 

DATA  XKKM 1/34975. 0,-50. 0,6975. 0,0. 0,300. 0/ 
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C  FIRST  DATA  FOR  SINGLE  ARRAY  TRACKING 

C  DATA  DATR/7500.0, 1300.0,0.0,-50.0,0.0,3*0.0,3*0.0 

C  +,4.712389, . 1745329,0. 1,8. 1,600.0,800.0/ 

C  DATA  XKKM1/7475. 0,-50. 0,1275. 0,0. 0,0.0/ 

C  SECOND  DATA  FOR  SINGLE  ARRAY  TRACKING 

C  DATA  DATR/2000. 0,1000. 0,300. 0,-50. 0,0.0, 3*0.0, 3*0.0 

C  +,4.712389, . 1745329,0. 1,8. 1,600.0,800.0/ 

C  DATA  XKKM1/1975. 0,-50. 0,975. 0,0. 0,300.0/ 

C  DATA  FOR  SUBROUTINE  QFIND 

DATA  SIGACC/36. 2/ ,SIGDI V/ 1.0/ , SIGCC/22. 2/ ,NZDIFF/4/ 
DATA  11/1/ , IJ/2/, IK/3/, IL/4/, IM/5/ ,MINE/1/ , JTIME/1 19/ 
C  OPEN  STATEMENTS  FOR  OUTPUT  FILES 
OPEN (4, FILE- 'TRUDI.DAT' > 

OPEN (13, FILE- 'PKK. DAT ' ) 

OPEN  < 1 1 , FILE- ' PKN. DAT ' ) 

OPEN (7 , FILE— ' XKK. DAT ' ) 

OPEN (8, FILE* ' XKN. DAT ' ) 

OPEN (9, FILE- ' XKERR. DAT ' ) 

OPEN (1 O , F I LE- ' XSERR . D AT ' ) 

OPEN ( 1 2 , F I LE- ' OUTPUT . DAT  ' ) 

C 

SIGCC  -  (SIGCC  *  3.141592654)  /  180.0 
C  TRANSPOSE  OF  GAMMA  MATRIX 

CALL  TRANS ( GAMMA , I M , I K , GAMMAT ) 

C  TRANSPOSE  OF  PHI  MATRIX 

CALL  TRANS (PHI , IM, IM,PHIT) 

USE  THESE  STATEMENTS  TO  CALCULATE  CONSTANT  Q  -  MATRIX 
CALL  PROD ( GAMMA , CO VW , I M , I K , I K , QTEMP > 

CALL  PROD ( QTEMP , GAMMAT ,IM,IK,IM,Q> 

I TIME  -  JTIME  +  1 
C  USE  THIS  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 
17  *  O 

C********************************************************** 
C  TIME  SLOTS  START  HERE 

DO  128  KK  -  1  ,  I TIME 

WRITE (*,562)  KK 

562  FORMAT (/, 10X, 'TIME  SLOT  IN  FILTERING  :',I5> 

C 

C&&S<&&&3<8c&8<S<&S<2(&&ScS<&S<&S<&8c8<8<&Sc&S<8<S<S<8<S<S(3<&Sc&S<S<S<S<8<8<S<8(3<S<S<S<S<8cS<S<S<!( 
C  CHOSE  THE  HYDROPHONE  ARRAY  FOR  MULTIPLE  ARRAY  TRACKING 

IF (XKKM1 ( 1 ) . GE. 33000. 0)  18  -  1 

IF ( ( XKKM1 (1) .GE. 27000.0) .AND. ( XKKM1 (1) .LT. 33000.0) ) 

+  18-2 

IF ( (XKKMl (1) .GE. 21000.0) .AND. ( XKKM1 (1) .LT. 27000.0) ) 

+  18  =  3 

IF ( (XKKMl (1) .GE. 15000.0) .AND. (XKKMl (1) .LT. 21000.0) ) 

+  18-4 

IF ( (XKKMl (1) .GE. 9000.0) .AND. (XKKMl (1) .LT. 15000.0) ) 

+  18  =  5 

IF( XKKMl (1) .LT. 9000.0)  18-6 


'.mn  l-v  « ‘^nr-n^r- T-'T^T-C*^-'  »  T-T-  T-.*-. 
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DO  205  13  =  1  ,  IL 

14  *  3  *  13 

15  a  14—2 

16  =14-1 
XB (13)  *  HYDRO  < IS , 15) 

YB (13)  =  HYDRO ( IS , 16) 

ZB  < 13)  *  HYDRO (13,14) 

CONTINUE 

C  WRITE  THE  COORDINATES  OF  CHOSEN  HYDROPHONE  ARRAY 
IF ( 17. NE. 18)  THEN 
WR I TE (*,217)  IB,KK 

12 


205 


217 


FORMAT (/, 1 OX, 'ARRAY  , 


STARTS  TRACKING  AT  TIME  ',13) 


WRITE (*,216)  KK, 18, ( 13, XB ( 13) ,  YB (13) 

+  ' , ZB (13) ,13  »  1  ,  IL) 

216  FORMAT (I5,I5,4<T11,I5,3(2X,D14.8) ,/) > 
17  =*  18 
END  IF 


»AAAAA 


\AAAAAAAAA 


\AAAAAAA A A A A 


C 

c 

c 

c 


USE 


CALCULATE  THE  TRUE  TIMES  AND  THE  TRUE  TRAJECTORY 
USE  THIS  CALL  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 
CALL  TRJC3 (KK , DATR , Z I , TD , XB , YB , ZB) 

THIS  CALL  STATEMENT  FOR  SINGLE  ARRAY  TRACKING 
CALL  TRAJEC  (  KK , DATR , Z I , TD ) 

A14  a  PHI (1 ,2) 

TRUX (KK)  *  TD  < 1 ) 

TRUY  <KK)  -  TD  (2) 

TRUZ  (KK)  »  TD  <3) 


WRITE (4,306)  KK , TRUX (KK) , TRUY (KK) ,TRUZ(KK> 


kAAAAAAAAAAA/ 


.-N  A  .  S  .a.  .A. 


C  *****S$S********S*S*SSSS**$$*SS*S**S*$********S*$*S3>£$$S$$ 
C  ROW  OF  H  -  MATRIX 

MINE  =  1 

163  DO  132  IROW  =  1  ,  IL 
NZDIFF  a  4 

C  USE  THIS  CALL  STATEMENT  TO  RUN  NOISE  SUBROUTINE 
CALL  NOISE (WINIT,WHTN) 

WHTN  a  (  i.o  /  3.0  )  *  WHTN 
C  ZERO  NOISE 
C  WHTN  =  0.0 

C  USE  THIS  CALL  STATEMENT  FOR  SINGLE  ARRAY  TRACKING 
C  CALL  CHROW ( I ROW , XKKM1 , HROW ) 

C  USE  THIS  CALL  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 
CALL  CHR0W3 ( I ROW , XKKM 1 , HROW , XB , YB , ZB) 

C ft#*#**#######*####*###*####*##*##*#*###########*######*#*# 
CG  C  K  3  =  C  P  C  K  /  K  -  1  3(5x5)  *  HT  <  5x  1 )  3  /  C  H(l>:5) 

C  *PCK/K  —  1  3(5x5)  *  HT(5xl)  +  COVC  V  3(1)  3 

CALL  MMULT <PKKM1,HR0W,IM,IM, GNUM ) 

CALL  VMUL  T ( HROW , GNUM , I M , GDT  EMP ) 
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GDENQM  =«  GDTEMP  +  CGW  <  I  ROW,  I  ROW) 

DO  134  IX  =  1  ,  IM 
GI (IX)  =  GNUM(IX)  /  GDENOM 

134  CONTINUE 

C########################################################## 

P  C  K  /  K  3  =  C  1(5x5)  -  G  C  K  3(5x1)  *  H  (1x5)  3- 

*  P  C  K  /  K  -1  3 

DO  135  IP  -  1  ,  IM 
DO  136  JP  -  1  ,  IM 

PDUM ( IP, JP)  -  (  -1.  *  GI (IP) )  *  HROW(JP) 
IF(IP.EQ.JP)  PDUM (IP , JP)  -  1.  +  PDUM ( IP , JP ) 

136  CONTINUE 

135  CONTINUE 

CALL  PROD ( PDUM , PKKM 1,IM,IM,IM,PI) 

CALCULATE  THE  PREDICTION  OF  MEASUREMENTS 
USE  THIS  CALL  STATEMENT  FOR  SINGLE  ARRAY  TRACKING 
CALL  CZHAT ( I ROW , XKKM1 , ZHAT) 

USE  THIS  CALL  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 
CALL  CZHAT3 ( IRON, XKKM1 , ZHAT, XB, YB, ZB) 

ZIC(IROW)  -  Z I ( I ROW)  +  WHTN  *  O.OOOOl 
ZD IFF ( I ROM)  -  Z IC ( I ROW)  -  ZHAT 
THREE  SIGMA  GATE 

P=DMAX1 ( DABS ( P I (1,1)) ,DABS(PI (3,3) ) ,DABS(PI (5,5) ) ) 
SIG=DSQRT(  (P/  (  (4860.  >**2)  >  +  (DABS  (COW  ( IROW ,  IROW)  >  )  ) 
GATE  -  3.0  *  SIG 

IF (KK. LE. 4)  GO  TO  149 

IF (DABS (ZDIFF( IROW) ) .LT. GATE)  GO  TO  149 
WRITE (*,147)  KK, IROW, GATE 

147  FORMAT (//, 10X, 'THREE  SIGMA  GATE  HAS  BEEN  EXCEEDED' 

+  , '  AT  TIME  ',14,'  IN  ROW  ',12,'  GATE  ;  ',D14.S) 

DO  148  LGJ  =  1  ,  IM 
GI (LGJ)  =0.0 

148  CONTINUE 
TAG  INVALID  TIME  MEASUREMENT 

ZD IFF ( IROW)  *  999. 


<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 

CX  CK/K3  =  XCK/K—  1  3+GCK3*CZCK3 
C  -ZCK/K-13  3- 


149 

150 

C<<<<< 


1 53 


DO  150  I  =  1  ,  IM 

XI (I)  =  XKKM1 ( I )  +  GI<I>  *  ZD IFF ( IROW) 
CONTINUE 

<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 

IF ( IROW. EQ. 4 )  GO  TO  152 
DO  153  I  =  1  ,  IM 
XKKMKI)  =  XI  (I) 

CONTINUE 


<  <  <  <  < 


DO  155  I  *  1  ,  IM 
DO  154  J  =  1  ,  IM 
PKKM1 < I , J)  »  PI < I , J) 

154  CONTINUE 

155  CONTINUE 
132  CONTINUE 

CS$S**SS$$S$*S**$*$S$****$******$*$$S*$****$S****$£*$$$$$$* 
152  DO  156  1=1,  IM 
XKK ( I )  =  XI (I) 

XKKM1 (I)  »  XI <I) 

C  USE  THIS  ADDITIONAL  STATEMENT  FOR  SMOOTHING 
XP6 ( I ,KK)  =  XI(I) 

C 

DO  157  J  =  1  ,  IM 
PKK  < I , J )  =  PI(I,J) 

PKKM1 (I , J)  *  PI (I , J) 

C  USE  THIS  ADDITIONAL  STATEMENT  FOR  SMOOTHING 
P5 ( KK , I , J )  =  PI (I ,J) 

C 


157  CONTINUE 
156  CONTINUE 

C»»>»>»»>»»»»»»»>»»>>>>»»>>>>>»  >>>>>>>»>> 

C  PREDICTION  OF  MEASUREMENTS  BASED  ON  XCK/KI 
DO  158  I  =  1  ,  IL 
C  EDIT  INVALID  TIME  MEASUREMENTS 

IF (ZD IFF  < I ) . GE. 999. )  GO  TO  159 
C  USE  THIS  CALL  STATEMENT  FOR  SINGLE  ARRAY  TRACKING 
C  CALL  CZHAT < I , XKKM 1 , ZHAT  > 

C  USE  THIS  CALL  STATEMENT  FOR  MULTIPLE  ARRAY  TRACKING 
CALL  CZHAT3  ( I , XKKM1 , ZHAT , XB, YB , ZB) 

ZD IFF  ( I )  =  DABSCZIC(I)  -  ZHAT) 

GO  TO  158 

159  ZDIFFCI)  -  0. 

NZDIFF  =  NZDIFF  -1 


158 


CONTINUE 


C  >  >»»»»>»  >  >»>  >  >  >  >  >  »»  >  >  >  >  >  >  >  >  »»»  >  >  >  >  >  >  >  >  >  >  >  >>>  >  > ; 

C  ABSOLUTE  AVERAGE  VALUE  OF  THE  DIFFERENCES  IN  MEASUREMENT 
IF (NZDIFF. EQ. 0)  GO  TO  160 

ZDIFTO  -  DABS  <  ZD IFF ( 1 ) +ZD IFF  <  2) +ZDIFF (3) + ZD IFF ( 4 )  ) 
ZDIFAV  =  ZDIFTO  /  NZDIFF 
IF (KK. LE. 4)  GO  TO  160 
IF (MINE. EQ. 1 )  SMTH(KK)  »  ZDIFAV 
IF (MINE. GT. 3)  GO  TO  160 
C  USE  THIS  CONSTANT  (2.0D-6)  GATE 

IFCZDIFAV.LT. 2. OD-6)  GO  TO  160 


C 


WRITE <*, 1473)  KK 

1473  FORMAT (//, 10X , ’CONSTANT  GATE  HAS  BEEN  EXCEEDED  AT’ 

+  , *  TIME  ’ , 14* 

C  ((((((((((((((((((((((((((((((((((((((((((((((((((((((((I  <, 

C  USE  THESE  STATEMENT  TO  INCREASE  THE  GAIN  IN  MULTIPLE  h 


v.v, 

vv 
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C  SINGLE  ARRAY  TRACKING 

SIGAAC  =  3.0  *  SIGACC 
SIGDDI  *  3.0  *  SIGDIV 
SIGCCC  =  3.0  #  SIGCC 

USE  THIS  CALL  STATEMENT  TO  CALCULATE  ADAPTIVE  Q-MATRIX 
CALL  QFIND(KK,XKK,PKK, SIGAAC, SIGDDI , S I GCCC , A 1 4 , Q ) 
CALL  ADD  <  PKK , Q , I M , I M , PKKM 1 ) 

MINE  -  MINE  +  1 
GO  TO  163 

C<  <<(<<<<<<<<<<<<<<(<<<<<<<<<<<<<<<<<<<<<<<<<<<(  <<<<<<<<<<  < 
160  MINE  =  1 

NZDIFF  =  4 
C 

WRITE  <7,301 )  KK , < XKK  <  J )  ,  J  =  1  ,  IM) 

WR I TE  <13, 30 1 )  KK,  <PKK<I , I)  , I  =  1  ,  IM) 

301  FORMAT  <I5,5<4X,D14.S> ) 

C 

XKERR  <KK)  =  XKK  < 1 )  -  TRUX <KK) 

YKERR  <KK)  -  XKK<3>  -  TRUY  <KK) 

ZKERR  <KK)  =»  XKK  (5)  -  TRUZ  (KK) 

C 

WRITE <9,306)  KK, XKERR (KK) , YKERR <KK) , ZKERR (KK) 

306  FORMAT  <15,3  <4X , D14. S) ) 

C  DETERMINE  MAX  &  MIN  ERRORS  AND  THE  TIME  SLOTS 
IF(KK.EQ.l)  THEN 
KX IK  =  KK 
KX2K  -  KK 
KY IK  »  KK 
KY2K  *  KK 
KZ IK  =*  KK 
KZ2K  =  KK 
XI KERR  »  XKERR <KK) 

X2KERR  »  XKERR <KK> 

Y1KERR  ■  YKERR <KK) 

Y2KERR  =  YKERR <KK) 

Z 1 KERR  »  ZKERR <KK) 

Z2KERR  »  ZKERR <KK) 

END  IF 

I F ( XKERR ( KK ) . GT . X 1 KERR )  KX1K  *  KK 
I F  < XKERR  < KK ) . GT . X 1 KERR )  X1KERR  =  XKERR  <KK) 

IF < XKERR (KK) .LT.X2KERR)  KX2K  =  KK 

IF  <  XKERR  <KK)  .LT.  X2KERR)  X2KERR  =  XKERR  (KK) 

IF (YKERR (KK) .GT.Y1KERR)  KY1K  =  KK 
IF (YKERR (KK) .GT.Y1KERR)  Y1KERR  =  YKERR (KK) 

IF (YKERR (KK) .LT.Y2KERR)  KY2K  =  KK 
IF (YKERR (KK) .LT.Y2KERR)  Y2KERR  =  YKERR (KK) 

I F ( ZKERR  < KK ) . GT . Z 1 KERR )  KZ1K  =  KK 

IF < ZKERR (KK) .GT. ZIKERR)  Z1KERR  =  ZKERR (KK) 

IF < ZKERR (KK) .LT. Z2KERR)  KZ2K  =  KK 
IF (ZKERR (KK) .LT. Z2KEP?)  Z2KERR  = 


ZKERR (KK) 
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C)  )))))))))))))))))))))))))))>))))))))))))))))))))))))))))  ) 
CPCK+l/K  3={PHI (5x5)  *  P  C  K  /  K  ] (5x5)  *  PHIT(5x5)> 
C  +•  Q  C  K  ] 

C  USE  THIS  CALL  STATEMENT  TO  CALCULATE  ADAPTIVE  Q-MATRIX 
CALL  OF I ND  <  KK ,  XKK , PKK , S I GACC , S I GD I V , S I GCC , A 1 4 , Q  > 

CALL  PROD (PHI , PKK, IM, IM, IM,PHIPKK) 

CALL  PROD ( PH I PKK , PH I T , I M , IM, IM,PKTEMP> 

CALL  ADD (PKTEMP , Q , IM , IM, PKKM1 ) 

C 

CALL  MMULT (PHI , XKK , IM, IM,XKKM1> 

C)  ))>>)))))))>>>)))>)>))>>)))>)>>)))))>)))))>)))))))>)))))  ) 
C  USE  THESE  STATEMENTS  FOR  SMOOTHING 
DO  302  IG  =  1  ,  IM 
XP ( IG,KK)  =  XKK(IG) 

CONTINUE 

DO  303  III  =*  1  ,  IM 
DO  304  JJJ  =  1  ,  IM 
SSI (KK, III , JJJ)  =  PKKM1 ( 1 1  I , JJJ) 

PI (KK, 1 1 1 , JJJ)  =  PKK (III, JJJ) 

CONTINUE 
CONTINUE 

C  SMOOTHING  STARTS  HERE 

I F  <  KK .  LE .  JT I  ME )  GO  TO  12S 
DO  500  K  =  1  ,  JTIME 
KI  =  JTIME  -  K  +  1 
WRITE (*,561 )  KI 

FORMAT (/, 10X,  ' IN  SMOOTHING  AT  TIME  :  ' , 15) 

DO  501  I  =*  1  ,  IM 
XPl(I)  =  XP6 ( I ,KI ) 

CONTINUE 

DO  502  I  =  1  ,  IM 
DO  503  J  =  1  ,  IM 
P2( I ,J)  =  P5 (KI , I , J> 

SS3 ( I , J)  =  SSI (KI,I ,J> 

IF (KI . LE. 4)  GO  TO  503 

IF(SMTH(KI) .GE.2.0D-6)  SS3(I,J)  =  3.6  *  SS3 ( I , J ) 
CONTINUE 
CONTINUE 

□  *******•**•*•*******■***■***■*******■*■*****■**■*****•*•*•*•*****•*■#■*•#•■*•#•■* 

C  A (K)  »  P (K/K)  *  TRANSPOSE I PH I  3  *  INVCP (K+l/K) 3 
CALL  TRANS (PHI, IM,IM,PHIT) 


561 


501 


503 

502 


CALL 

CALL 

CALL 

CALL 


RECIP (SS3 , IM , SS3R  > 


PROD ( SS3 , SS3R , IM, IM, IM,CH) 

PROD  < PH I T , SS3R , IM, IM, IM, TEMPI ) 

PR0D(P2, TEMPI , IM, IM, I M , AK ) 

C***********-* **#*#***************•»-»-»*-»' 4*.************* 

C  X (K/N)  =  X (K/K)  +  A ( K )  *  C  X(K+1/N)  -  X(K+1/K)  3 

DO  504  I  =  1  ,  IM 

XNNM1 ( I )  =  XP ( I , KI+1 > 

504  CONTINUE 


%  , 

fcv 


£■ 


94 


.  ^ 


■  „ « . « . «  ,  • ,  ■  •  ,  •  * .  >  »  ,  •  .  * ,  •  -  ,  • 


CALL  MMULT  < PH I ,XP1 , I M , I M , SS2 ) 

CALL  SUB  (  XNNM 1  ,  SS2  ,  I M ,  1  ,  TEMP2 ) 

CALL  PROD ( AK , TEMP2 ,  I M ,  I M  ,  1  , TEMP3 ) 

CALL  ADD ( XP1 , TEMP3 , IM , 1 , XKKS) 

DO  505  I  =  1  ,  IM 
XP  ( I ,KI )  =  XKKS (I) 

505  CONTINUE 

C$$*SS$$**S**$$$$*SS$*SSSS***$$**$*S$$**$*S******££S$$$£S£$ 
WRITE (S , 30 1 >  KI , ( XKKS  (  J )  ,  J  =  1  ,  IM) 

XSERR (KI)  =  XKKS ( 1 )  -  TRUX(KI) 

YSERR (KI)  =  XKKS (3)  -  TRUY (KI ) 

ZSERR (KI)  =  XKKS  (5)  -  TRUZ(KI) 

WRITE < 10,306)  KI , XSERR (KI ) , YSERR (KI ) , ZSERR (KI) 

C  DETERMINE  MAX  &  MIN  ERRORS  AND  THE  TIME  SLOTS 
IF(K.EQ.l)  THEN 


KX  IS  = 

KI 

KX2S  = 

KI 

KYIS  = 

KI 

KY2S  = 

KI 

KZ  IS  = 

KI 

KZ2S  = 

KI 

XI SERR 

=»  XSERR  (KI) 

X2SERR 

=  XSERR (KI) 

Y 1 SERR 

=  YSERR (KI) 

Y2SERR 

=  YSERR (KI) 

Z1SERR 

*  ZSERR (KI) 

Z2SERR 

*  ZSERR (KI) 

END  IF 

IF ( XSERR (KI ) . GT. X 1SERR)  KX1S  *  KI 

IF  <  XSERR (K I ) . GT. X 1SERR)  X1SERR  =.XSERR(KI> 

IF (XSERR (KI ) .LT. X2SERR)  KX2S  =  KI 
IF (XSERR (KI ) .LT.X2SERR)  X2SERR  =  XSERR (KI) 

I F ( YSERR ( K I )  . GT . Y 1 SERR )  KYIS  =  KI 
IF (YSERR (KI ) .GT.Y1SERR)  Y1SERR  =  YSERR (KI) 

IF (YSERR (KI ) .LT.Y2SERR)  KY2S  =  KI 
IF (YSERR (KI ) .LT.Y2SERR)  Y2SERR  =  YSERR (KI) 

IF ( ZSERR (KI )  . GT. Z 1 SERR)  KZ1S  =  KI 

IF ( ZSERR (KI ) . GT. Z 1SERR)  Z1SERR  =  ZSERR(KI) 

IF ( ZSERR (KI ) . LT. Z2SERR )  KZ2S  =  KI 

IF ( ZSERR (KI ) .LT. Z2SERR)  Z2SERR  =  ZSERR (KI) 

C  P ( K / N )  =  P (K/K) +A (K) *CP(K+1/N) — P (K+l/K) 3 *TRANSPOSEC A (K )  ] 
DO  506  I  =  1  ,  IM 

DO  507  J  =  1  ,  IM 
PNNM1 < I  ,  J )  =  P1(KI  +  1,I,J) 

507  CONTINUE 

506  CONTINUE 

CALL  SUB (PNNM1 ,SS3 , IM , IM , TEMP4 ) 

CALL  TRANS ( AK , I M , I M , AKT ) 

CALL  PROD (TEMP4, AKT, IM, IM, IM,TEMP5) 

CALL  PROD (AK, TEMPS, IM, IM, IM, TEMP6> 


CALL  ADD  <  P2 , TEMP6 , I M , I M , PKKS ) 

DO  508  I  »  1  ,  IM 
DO  509  J  »  1  ,  IM 
PI (KI,I,J)  »  PKKS  ( I ,  J ) 

CONTINUE 
CONTINUE 

$ 

WRITE (11, 301)  K I , < PKKS < I , I ) , I  -  1  ,  IM) 
C*******************#****#**#**#*#*■**,,,'■**',H,''**'*,,'*',H*',H,■*',,',,,,'*'*''*','‘',*'‘,'’ 
500  CONTINUE 


CONTINUE 
WRITE ( 12,800) 

FORMAT (10X,‘  T  IME ' , 4  X , 


FORMAT (10X,‘  T IME ' , 4  X ,  MAX.  ERROR  ',4X,'  TIME',4X 

+  , '  MIN.  ERROR  ' ) 

WRITE (12,801)  KX 1 K , X 1 KERR , KX2K , X2KERR , KY 1 K , Y 1 KERR , 
+KY2K , Y2KERR ,KZ1K,Z1KERR,KZ2K,Z2KERR 
WRITE( 12,801)  KX IS , X 1SERR , KX2S , X2SERR , KYIS , Y1SERR , 
+KY2S , Y2SERR , KZ 1 S , Z 1 SERR , KZ2S , Z2SERR 
FORMAT (3(/, 10X,I5,4X,D14.8,4X, I5,4X,D14.8) ) 

STOP 
END 

SUBROUTINE  TRANS (AA, NR ,NC , BB) 

REAL *8  AA ( NR , NC ) , BB ( NC , NR ) 

DO  3  I  »  1  ,  NR 
DO  30  J  =  1  ,  NC 
BB ( J  ,  I )  »  AA ( I , J ) 

CONTINUE 
CONTINUE 
RETURN 
END 


SUBROUT I NE  PROD ( AA , BB , NRA , NCA , NCB , CC  > 

REAL *8  AA ( NRA , NCA ) , BB  <  NCA , NCB )  , CC ( NRA , NCB  > 
DO  4  I  »  1  ,  NRA 

DO  40  J  »  i  ,  NCB 

CC(I,J)  »  o.O 
CONTINUE 
CONTINUE 

DO  41  I  =  i  ,  NRA 

DO  410  J  =  1  ,  NCB 
DO  411  K  =  1  ,  NCA 

CC(I,J)  =  CC(I,J)  +  AA  < I , K )  *  BB (K , J ) 
CONTINUE 
CONTINUE 
CONTINUE 
RETURN 
END 

SUBROUTINE  TRAJEC <KK , DATR, Z I , TD) 


+  AA  < I , K )  *  BB (K , J ) 


aV'V''aV!'>Iv\-'v'vVV' 
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REAL *8  DATR (17) ,21 (4) ,TD(3) , COEFF , RANGE ,VEL, T 
DATA  VEL/4860.0/, I IK/3/ , I IM/5/ 

T  -  0.0 

COEFF  =  1.0  /  VEL 

2 1 <1 ) =COEFF*OSQRT < ( (DATR ( 1 > +15. 0) **2) 

+  +  (  < DATR  ( 2 )  + 1 5 .  0 ) **2 )  +  ( (DATR (3) +15. 0) **2) ) 

21  (2)  =*COEFF*DSQRT  (  <  (DATR ( 1 ) -15. 0) **2> 

+  +( < DATR ( 2 ) + 1 5 . 0 >  **2 )  +  ( (DATR (3) +15. 0) **2) ) 

21 (3)-C0EFF*DSQRT(  < (DATR ( 1 ) +15.  0) **2> 

+  +(  (DATR (2)  —15.0)  **2)  +  (  (DATR  (3) +15.  0)  **2)  ) 

21 (4)-C0EFF*DSQRT( ( (DATR ( 1 ) +15.  0) **2> 

+  +( ( DATR ( 2 ) + 1 5 . 0 ) **2 ) + ( (DATR (3) -15. 0) **2) ) 

DO  5  I  »  1  ,  IIK 
TD  ( I )  »  DATR  (I.) 

5  CONTINUE 

USE  THIS  STATEMENT  FOR  STRAIGHT  RUN 

IF ( (KK. LE. DATR ( 17) ) . AND. (KK. GT. DATR ( 16) ) )  GO  TO  50 
USE  THESE  STATEMENTS  FOR  MANEUVERING  RUN 
58  IF( (KK.LE.49) .AND. (KK.GT.22) )  GO  TO  50 
IF( (KK.LE.98) . AND. (KK.GT.71) >  GO  TO  50 
IF ( (KK. EQ. 50) .OR. (KK.EQ.99) )  THEN 

FIRST  DATA  FOR  TRUE  TRAJECTORY  IN  SINGLE  ARRAY  TRACKING 
DATR (2)  ■  1300.0 
DATR (3)  »  0.0 

SECOND  DATA  FOR  TRUE  TRAJECTORY  IN  SINGLE  ARRAY  TACKING 


DATR (2)  = 
DATR (3)  * 
DATR (4)  * 
DATR (5)  = 
DATR (6)  » 
DATR (7)  = 
DATR (8)  = 
DATR (9)  = 
DATR (10) 
DATR (11) 
DATR (12) 
DATR (13) 
DATR (14) 
DATR (15) 
END  IF 

DATR (7)  = 
DATR (8)  - 
DATR (14)  * 
GO  TO  51 
DATR (14)  * 
DATR (12)  = 
DATR (7)  = 
DATR (8)  - 


1000.0 
300.0 
-50.0 
0.0 
0.0 
0.0 
0.0 
0.0 
-0.0 
■  0.0 

■  4.712389 

■  0. 1745329 
-0.  1 

-  8.  1 


0.0 
0.0 
'  1.31 

'  0.005 
■  DATR (12) 
DATR (15) 
DATR (15) 


h  DATR (13)  *  DATR (14) 
DCOS (DATR (12) ) 

DSIN (DATR (12) ) 


51 


DO  52  I  -  1  ,  I IM 

DATR(I>  -  DATR(I)  +  DATR(I+3>  *  DATR (14) 

+  ( ( (DATR(14> )**2)/2)  *  DATR (1+6) 

52  CONTINUE 

T  »  T  +  DATR (14) 

IF (DABS (T  -  1.31) -LE. O.OOOl)  RETURN 
00  TO  53 
END 
C 

SUBROUTINE  TRJC3 (KK ,DATR, Z I , TD, XB , YB, ZB) 

REAL *8  DATR (17) , Z I (4) , TD (3) , XB (4) , YB (4) , ZB <4> , COEFF 
REAL *8  VEL , T 

DATA  VEL/ 4860. 0/ , I  IK/3/ , I IL/4/ , I IM/5/ 

T  =  0.0 

COEFF  ■  1.0  /  VEL 
DO  12  I  -  1  ,  II L 

ZI(I)  ■  COEFF  *  DSQRT ( ( (DATR  < 1 )  -  XB(I))**2) 

+•  +  <  (DATR  (2)  -  YB  ( I )  )  **2)  +•  ((DATR  (3)  -  ZB(I))**2)> 

12  CONTINUE 

DO  120  1=1  ,  I IK 
TD ( I )  =  DATR ( I ) 

120  CONTINUE 

C  USE  THIS  STATEMENT  FOR  STRAIGHT  RUN 

IF ( (KK. LE. DATR ( 17) ) . AND. (KK. 8T. DATR ( 16) ) >  GO  TO  121 
C  USE  THESE  STATEMENTS  FOR  MANEUVERING  RUN 
C  128  IF ( (KK. LE. 49) . AND. (KK. GT. 22) )  GO  TO  121 
C  IF( (KK.LE.98) .AND. (KK.GT.71) )  GO  TO  121 

C  IF ( (KK. EQ. 50) . OR. (KK. EQ. 99) )  THEN 

C  DATR (2)  =  7000.0 

C  DATR (3)  =  300.0 

C  DATR (4)  =  -50.0 

C  DATR (5)  =0.0 

C  DATR (6)  =0.0 

C  DATR (7)  =0.0 

C  DATR (8)  =0.0 

C  DATR (9)  =0.0 

C  DATR (10)  =  0.0 

C  DATR (11)  =0.0 

C  DATR (12)  =  4.712389 

C  DATR (13)  =  0.1745329 

C  DATR (14)  =  0. 1 

C  DATR (15)  =  8. 1 

C  END IF 

C 

127  DATR (7)  =0.0 
DATR (8)  =0.0 
DATR (14)  =  1.31 
GO  TO  122 

121  DATR (14)  =  0.005 

124  DATR  (12)  =  DATR  (12)  DATR  (13)  *  DATR  (14) 

DATR (7)  =  DATR (15)  *  DCOS (DATR < 12) ) 


DATR <8)  =  DATR (15)  *  DSIN (DATR < 12) ) 

122  DO  123  I  ■  1  ,  IIM 

DATR ( I )  -  DATR ( I )  +  DATR < 1+3)  *  DATR (14) 

+  (  (  (DATR  (14)  )**2)  /2)  *  DATR  (1+6) 

123  CONTINUE 

T  -  T  +  DATR (14) 

IF (DABS (T  -  1.31) .LE. O.OOOl)  RETURN 

GO  TO  124 

END 

SUBROUTINE  CHROW ( I ROW, XKKM1 ,HROW> 

REAL*8  HROW ( 5 ) , XKKM 1(5), COEFF , DENOM , DENOM 1 , DEN0M2 
REAL *8  VEL , A 1 , A2 , A3 , DEN0M3 , DEN0M4 
DATA  VEL/4860. QJ 
COEFF  -  1.0  /  VEL 

DENOM 1«DSQRT  (  (  (XKKM1  ( 1 )  4-15.0)  **2)  4-  (  ( XKKMl  (3)  +1S.  O)  *•*2) 
+  +(  (XKKMl  (5)4-15.0)  **2)  ) 

DENOM2-DSQRT (  <  (XKKMl  ( 1 ) -13.  O)  **2)  +  (  (XKk'Ml  (3)  4-15.  0)  **2) 
+  4> (  ( XKKM  1  (5)4-15. 0)  **2 )  > 

DEN0M3-DSQRT  (  (  (XKKM1  ( 1 )  4-15. 0)  **2)  4- (  ( XKKM1  (3) -15.  O)  **2> 
4-  4-  (  <  XKKM1  (5)4-15.0)  **2)  ) 

DEN0M4-DSQRT  (  (  ( XKKM1  ( 1 )  4-15. 0)  **2)  (  ( XKKM1  (3)  4-15.  O)  **2) 

4-  4-(  (XKKM1  (5) -15. 0)  *442)  ) 

A1  -  1.0 
A2  »  1.0 
A3  —  1.0 
DENOM  -  DENOM 1 

IF ( IRON. EQ. 2)  DENOM  -  DEN0M2 
IF ( IRGW. EQ. 3)  DENOM  »  DEN0M3 
IF ( I ROW. EQ. 4)  DENOM  -  DEN0M4 
IF ( IROW. EQ. 2)  A1  -  -  1.0 

HROW(l)  -  COEFF  *  (  (XKKM1  ( 1 )  4-  A1  *  15.0)  /  DENOM) 

IF ( IROW. EQ. 3)  A2  -  -  1.0 

HROW  ( 3 )  -  COEFF  *  (  ( XKKM1  (3)  4-  A2  *  15.0)  /  DENOM) 

IF ( IROW. EQ. 4)  A3  «  -  1.0 

HROW  ( 5 )  -  COEFF  *  (  (XKKMl  (5)  4-  A3  *  15.0)  /  DENOM) 

HROW (2)  -  0.0 
HROW (4)  »  0.0 
RETURN 
END 

SUBROUTINE  CHR0W3 ( IROW, XKKMl , HROW , XB , YB , ZB) 

REAL *8  HROW ( 5 ) , XKKMl (5) , COEFF , DENOM , VEL , XB ( 4 ) ,YB(4) 
REAL *8  XO, YO, ZO, ZB ( 4) 

DATA  VEL/4860. 0/ 

COEFF  «  1.0  /  VEL 
XO  -  XB ( IROW) 

YO  -  YB ( IROW) 

ZO  =  ZB (IROW) 

DENOM  »  DSQRT(  (  (XKKMl  <l)-X0>**2)4-(  (XKKMl  (3>-Y0)**2) 

4-  4-  (  (XKKMl  (5)-Z0>**2)  > 


XO)  /  DENOM) 


HROW ( 1 )  -  COEFF  *  <<XKKM1<1)  - 
HROW ( 2 )  »  0.0 
HROW ( 3 )  »  COEFF  *  ( (XKKMl <3)  -  Y0>  /  DENOM) 

HROW (4)  -0.0 

HROW (5)  =  CnEFF  *  ( ( XKKMl (5)  -  ZO)  /  DENOM) 

RETURN 

END 

SUBROUT I NE  MMULT (AA,BB, NR A , NCA , CC ) 

REAL *8  AA (NR A, NCA) , BB (NCA) ,CC(NRA> 

DO  6  I  >  1  ,  NRA 
CC(I>  =  0.0 
DO  60  J  a  1  ,  NCA 
CC(I)  -  CC(I)  +  AA(I,J>  *  BB(J) 

CONTINUE 

CONTINUE 

RETURN 

END 

SUBROUTINE  VMULT ( AA, BB ,NE,CC> 

REAL *8  AA(NE) , BB(NE) , CC 
CC  =  0.0 
DO  7  I  =  1  ,  NE 
CC  *  CC  +  AA(I)  *  BB(I) 

CONTINUE 

RETURN 

END 

SUBROUTINE  CZHAT ( IROW, XKKM1 , ZHAT) 

REAL *8  XKKM1 ( 5 ) , ZHAT , COEFF , VEL 
DATA  VEL/ 4860.0/ 

COEFF  -  1.0  /  VEL 

IF  ( IROW.  EQ.  1)  ZHAT=»COEFF*DSQRT  (  (  ( XKKM1  ( 1 )  +  15. 0)  **2)  + 
+  ( (XKKM1 (3)+15.0)**2)+(  (XKKMl (5) +15.  0)  **2) ) 

IF  ( IROW.  EQ.  2)  ZHAT=COEFF*DSQRT  (  (  (XKKM1  ( 1 > -15. 0) **2) + 
+  ( (XKKM1 (3)+15.0)**2)+( ( XKKM1 (5) +15. O) **2) ) 

IF ( IROW. EQ. 3)  ZHAT=COEFF*DSQRT ( ( ( XKKM1 ( 1 ) +15.  0) **2)  + 
+  ( ( XKKM1 (3) -15.0) **2>+( (XKKM1 (5) +15. O) **2) ) 

IF ( IROW. EQ. 4)  ZHAT-COEFF*DSQRT ( <  < XKKM1 ( 1 ) +15. O) **2)  + 
+  ( (XKKM1 ( 3 ) + 1 5 . 0 ) **2 ) + ( (XKKM1 (5) -15. O) **2) ) 

RETURN 
END 

SUBROUTINE  CZHAT3 ( IROW , XKKM1 , ZHAT , XB , YB , ZB) 

REAL *8  XKKM1 (5) , ZHAT , COEFF , VEL , XB ( 4) , YB (4) , ZB (4) , XO 
REAL*8  YO, ZO 
DATA  VEL/ 4860.0/ 

COEFF  =  1.0  /  VEL 
XO  =  XB ( IROW) 

YO  =  YB ( IROW) 

ZO  »  ZB (IROW) 


ZHAT  «  COEFF  *  DSQRT (  (  < XKKM1  <  1 )  -  X0)**2)  + 

<  (XKKMl  (3)  -  Y0)*+2>  +  <  (XKKM1  (5)  -  Z0)**2)) 

RETURN 

END 


SUBROUTINE  NOISE<R,P) 

REAL *8  Y  <  6 ) ,  X  <  6 ) , S ( 5 ) , R , P , BB , P 1 
DATA  Y/O.O,. 0228,. 0668,. 1357,. 27 43,. 5/ 

DATA  X/-3. 01 , -2. 0,-1. 5, -1.0, -0.6, 0.0/ 

DATA  S/ 43 . 8596 ,11. 3636 , 7 . 25689 ,2.891 352 , 2 . 65887 / 

BB  *  1.0 
PI  *  R  *  317.0 
R  »  DMOD <P1  ,  BB) 

P  *  R 
I  -  1 

IF(P.GT.O.S)  P  »  1.0  -  R 
IF (P. LT. Y  <  1  +  1 ) )  GO  TO  80 
1*1  +  1 
GO  TO  8 

P  ■  ( (P  —  Y ( I ) )  *  S(I)  +X(I>> 

IF (R. GE. 0.5)  P  *  -  P 
RETURN 
END 

SUBROUTINE  ADD  < AA , BB , NR , NC , CC ) 

REAL*8  A A  <  NR , NC ) , BB  <  NR , NC ) ,CC(NR,NC) 

DO  9  I  -  1  ,  NR 
DO  90  J  *  1  ,  NC 
CC(I,J>  «  AA ( I , J )  +  BB ( I , J ) 

CONTINUE 
CONTINUE 
RETURN 
END 

SUBROUTINE  QFIND (KK , XKK , PKK , SIGACC, SIGDI V, SIGCC , A , Q) 

REAL*8  XKK (5) ,PKK<5,5) ,Q<5,5) , SIGACC, SIGDIV, SIGCC. A 
REAL*8  A2 , A3 ,B,C,D,E1 ,E12,E2,G1 ,G2,G3,SIGAAC.SIGDDI 
REAL *8  S I GCCC , A 1 
INTEGER  KK 

IF (KK. NE. 1)  GO  TO  111 
DO  11  I  -  1  ,5 
DO  110  J  *  1  ,  5 
Q  < I , J)  *  0.0 
CONTINUE 
CONTINUE 

SIGACC  -  SIGACC  **2 
Q (5,5)  »  (SIGDIV  *+2)  *  (A  **2) 

SIGCC  *  SIGCC  **2 
G1  *  <  A  +*2  )  /  2.0 

G"’  *  G1  *+2  ,n>' s'. 

G  »  A  *  G1  <>&& 


»  A  **2 

»  XKK<2>  **2  +  XKK  < 4 )  **2 
»  XKK  <2)  /  DSQRT(Al) 

XKK (4)  . 

XKK  <4)  /  DSQRT  < A1 ) 

XKK (2) 


El  =  (  A3  **2 )  *  SIGACC  (  B  **2  )  *  SIGCC 

E12  »  A3  *  C  *  SIGACC  -  B  *  D  *  SIGCC 

E2  *  <  C  **2  )  *  SIGACC  +  <  D  **2  )  *  SIGCC 

Q  ( 1 , 1 )  *  El  *  G2 
Q ( 1 ,2)  =  G3  *  El 
Q  < 1 ,3)  =  E12  *  G2 
Q ( 1 , 4)  »  G3  *  E12 

Q  (2,2)  =»  A2  *  El 

□ (2,3)  ■  G3  *  E12 

Q (2,4)  *  A2  *  El 2 

Q (3,3)  »  G2  *  E2 

Q (3,4)  =  G3  *  E2 

Q(4,4)  ■  A2  *  E2 

DO  112  I  ■  1  ,4 
DO  113  J  -  1  ,  I 
(3(1, J)  =  Q  ( J  ,  I ) 

CONTINUE 
CONTINUE 
RETURN 
END 

SUBROUTINE  SUB (AA,BB,NR,NC,CC) 

REAL *8  A A ( NR , NC ) , BB (NR, NC) ,CC(NR,NC) 

DO  12  I  ■  1  ,  NR 
DO  120  J  -  1  ,  NC 
CC(I,J)  -  AA ( I , J )  -  BB ( I , J ) 

CONTINUE 
CONTINUE 
RETURN 
END 

SUBROUTINE  RECIP (AA,NN,CC) 

REAL*8  A A ( NN , NN ) ,DD<5, 10) ,CC(NN,NN) 

DO  14  K  *  1  ,  NN 
DO  140  J  »  1  ,  NN 
DD (K, J)  =  A A ( K , J ) 

CONTINUE 
CONTINUE 

DO  141  K  =  1  ,  NN 
I  ■  K  ♦  NN 
DO  142  J  =*  6  ,  10 
IF(I.NE.J)  GO  TO  143 
DD (K , J )  =  1. 

GO  TO  142 
DD  (K  ,  J )  =  O. 


APPENDIX  C 

PLOTTING  PROGRAM  LISTING  FOR  HP  PLOTTER 


♦STORAGE: 2 
♦DEBUG 
♦NOLI ST 
C 

PROGRAM  PLOTTER 
C 

CHARACTER *40  TITLE 
CHARACTER*35  LEGEND, SUBTITLE 
CHARACTER *25  NAMEX ,NAMEY 
REAL  X  <245) , Y  <245) 

REAL  0 <245) ,P<245> ,R<245> ,SC245) ,TC245>  ,U<245) 
INTEGER *2  IC 
DATA  IC/O/ 

C 

C  USE  THESE  FOR  MULTIPLE  ARRAY  TRACKING 

C  TITLE  ™ ' MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN 

TITLE  =  ' MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUN ' 

C 

C  USE  THESE  FOR  SINGLE  ARRAY  TRACKING 

C  TITLE  «' SINGLE  ARRAY  ADAPTIVE  MANEUVERING  RUN' 

C  TITLE  - 'SINGLE  ARRAY  ADAPTIVE  STRAIGHT  RUN' 

OPEN <5 , F ILE™ ' XKK . DAT ' , STATUS- ' OLD ' ) 

DO  32  LENG  -  1  ,  241 

READ  < 5 ,  * , END=»33 )  0  < LENG )  , P  < LENG )  ,  R  < LENG )  ,  S  (LENG) 
+  T (LENG) ,U (LENG) 

32  CONTINUE 

33  CONTINUE 

LENG  =  LENG  -  1 
CLOSE ( 5 , STATUS™  '  KEEP  '  > 

NAMEX  » ' XCK/KD  (FT)  ' 

NAMEY  * ' YCK/Kl  (FT)' 

SUBTITLE  ™' 

LEGEND  ™ 'FILTERED  ESTIMATE  OF  TRAJECTORY' 

CALL  DRAWER ( T I TLE , NAME X , NAMEY , LEGEND , P , S , LENG , 

+  SUBTITLE) 

OPEN ( 5 , F I LE™ ' PKK . DAT ' , STATUS™ 'OLD ' ) 

DO  34  LENG  ™  1  ,  241 

READ  < 5 , * , END=35 )  0 (LENG)  , P (LENG)  ,R (LENG)  , S (LENG)  , 
+■  T  (LENG)  ,U  (LENG) 

34  CONTINUE 

35  CONTINUE 

LENG  ™  LENG  -  1 
CLOSE  <  5 , STATUS™ ' KEEP ' ) 

NAME  X  = ' T I ME  SLOTS' 

NAMEY  - ' <FT**2) ' 


SUBTITLE  * ' PCK/K 1(1,1) 

LEGEND  =  'FILTERED  ERROR  COVARIANCE  P( K/K)  ' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND , 0 , P , LENG , 

SUBTITLE) 

NAMEY  »' <  <FT /SEC) **2)  * 

SUBTITLE  -'PCK/K3 (2,2) ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND ,0,R, LENG , 

SUBTITLE) 

NAMEY  «' (FT**2) ' 

SUBTITLE  -'PCK/K3 (3,3) ' 

CALL  DRAWER (TITLE, NAME X , NAMEY , LEGEND , 0 , S , LENG , 

SUBTITLE) 

NAMEY  - '  (  (FT/SEC)  **2)  ' 

SUBTITLE  -  PCK/.K3  (4,4)  ' 

CALL  DRAWER ( T I TLE , NAME X , NAMEY , LEGEND , 0 , T , LENG , 

SUBTITLE) 

NAMEY  =  ' (FT**2) ' 

SUBTITLE  -  'PCK/KJ  (5,5)  ' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND , 0 , U , LENG , 

SUBTITLE) 

OPEN (5,FILE«' XKERR.DAT' , STATUS- 'OLD ' ) 

DO  38  LENG  *  1  ,  241 

READ (3,*, END-39)  0 (LENG) ,P (LENG) , R (LENG) , S (LENG) 

CONTINUE 

CONTINUE 

LENG  -  LENG  -  1 

CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEY  -X  ERROR  (FT) ' 

SUBTITLE  ■' 

LEGEND  » 'ERROR  IN  FILTERED  ESTIMATE' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND , O , P , LENG , 

SUBTITLE) 

NAMEY  — ' Y  ERROR  (FT) ' 

CALL  DRAWER (TITLE, NAME  X , NAMEY , LEGEND , O , R , LENG , 

SUBTITLE) 

NAMEY  »'Z  ERROR  (FT)  ' 

CALL  DRAWER ( T I TLE , NAME X , NAMEY , LEGEND , O , S , LENG , 

■  SUBTITLE) 

OPEN ' 5 , F I LE« ' XKN. DAT ' , STATUS- ' OLD ' ) 

DO  40  LENG  -  1  ,  241 

READ(5, *, END-41 >  0 (LENG) , P (LENG) , R (LENG) , S (LENG) 

T(LENG) ,U(LENG) 

CONTINUE 

CONTINUE 

LENG  =  LENG  -  1 

CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEX  - ' X CK/N3  (FT)  ' 

NAMEY  - ' YCK/N3  (FT) ' 

LEGEND  -'SMOOTHED  ESTIMATE  OF  TRAJECTORY' 

CALL  DRAWER (TITLE, NAMEX , NAMEY , LEGEND , P , S , LENG , 

SUBTITLE) 


OPEN ( 5 , F I LE- ' PKN. DAT ' , STATUS- ' OLD ' > 

DO  42  LENG  =  1  ,  241 

READ <5,*, END-43)  0 (LENG) , P (LENG) , R (LENG) ,  S (LENG) 

T (LENG) ,U (LENG) 

CONTINUE 

CONTINUE 

LENG  -  LENG  -  1 

CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEX  -'TIME  SLOTS' 

NAMEY  -' ( FT **2 ) ' 

SUBTITLE  -'PCK/NI (1,1) ' 

LEGEND  -'SMOOTHED  ERROR  COVARIANCE  P(K/N)' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG , 

SUBTITLE) 

NAMEY  -' ( (FT/SEC) **2) ' 

SUBTITLE  - '  PCK/N3 (2,2) ' 

CALL  DRAWER ( T I TLE , NAME  X , NAME Y , LEGEND , O , R , LENG , 

SUBTITLE) 

NAMEY  -' ( FT **2 ) ' 

SUBTITLE  -PCK/NI (3,3) ' 

CALL  DRAWER ( T I TLE , N AME  X , NAMEY , LEGEND , 0 , S , LENG , 

SUBTITLE) 

NAMEY  -' ( (FT /SEC) **2>  ' 

SUBTITLE  — 'PCK/NI (4,4) 

CALL  DRAWER (TITLE, NAME  X , NAMEY , LEGEND , O , T , LENG , 

SUBTITLE) 

NAMEY  -'  ( FT  **2. )  ' 

SUBTITLE.  -'PCK/NI (5,5)  ' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND , O , U , LENG , 

SUBTITLE) 

OPEN ( 5 , F I LE- ' XSERR . DAT ' , STATUS- ' OLD ' ) 

DO  44  LENG  =  1  ,  241 

READ (5,*, END-45)  0 (LENG) , P (LENG) , R ( LENG ) , S (LENG) 

CONTINUE 

CONTINUE 

LENG  -  LENG  -  1 

CLOSE (5 , STATUS- ' KEEP ' ) 

NAMEY  -'X  ERROR  (FT)' 

SUBTITLE  -' 

LEGEND  -'ERROR  IN  SMOOTHED  ESTIMATE' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND , 0 , P , LENG , 

SUBTITLE) 

NAMEY  — ' Y  ERROR  (FT) ' 

CALL  DRAWER ( T I TLE , NAME X , NAMEY , LEGEND ,Q,R , LENG , 

SUBTITLE) 

NAMEY  -Z  ERROR  (FT) ' 

CALL  DRAWER ( T I TLE , NAME X , NAME Y , LEGEND , O , S , LENG , 

SUBTITLE) 

STOP 

END 

SUBROUT I NE  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , X , Y , 


n  n  n  n  n 


LENG, SUBTITLE) 


CHARACTER*40  TITLE 
CHARACTER*35  LEGEND, SUBTITLE 
-  CHARACTER*25  NAMEX ,NAMEY 
REAL  X  <245) , Y (245) 

INTEGER*2  IC 
DATA  IC/O/ 

CALL  ED 
CALL  CUP (1,0) 

CALL  PLOTS (0,9600,30) 

CALL  SYMBOL (2.0,6. 65 , . 20 , T I TLE ,0.0, 40 ) 

CALL  SYMBOL (2.  0,6.  25,. 1 75 , LEGEND , O. 0 , 35 ) 

USE  THIS  FOR  NOISELESS  TRACKING 

CALL  SYMBOL (6. 84, 6. 25, . 175, 'WITHOUT  NOISE' ,0.0,13) 

USE  THIS  FOR  NOISY  TRACKING 

CALL  SYMB0L(6. 84,6. 25,. 175, '  WITH  NOISE ' ,0. O, 13) 

C 

CALL  SYMBOL ( 1 . 60 , 2 . 45 , . 20 , SUBT I TLE ,90.0,35) 

CALL  PLOT (1.00, 1.00, -3) 

CALL  PLOT (8. 0,0. 0,3) 

CALL  PLOT (8. 0,6. 0,2) 

CALL  PLOT (0.0,6. 0,2) 

CALL  PLOT (0.0, 0.0, 2) 

CALL  PLOT (8. 0,0. 0,2) 

CALL  SCALE (X, 6. 00, LENG, 1) 

CALL  SCALE(Y, 3.00, LENG, 1) 

CALL  STAX  IS ( . 180 , . 20 , . 15 , . 112,-1) 

CALL  AXIS (1.5, 1.5, NAMEX, -13, 6. 00, 00. ,X(LENG+1) , 

+  X (LENG+2) ) 

CALL  STAXIS(. 15, .20,. 111,. 112,2) 

CALL  AX IS (1.5, 1.5, NAMEY, 13,3.00,90. , Y  < LENG+ 1 )  , 

+  Y (LENG+2)) 

CALL  PLOT ( 1 . 50 , 1 . 50 , —3 ) 

CALL  LINE(X, Y, LENG, 1 ,0,3) 

CALL  PL0T(0. 0,0. 0,999) 

RETURN 

END 

SUBROUTINE  ED 
CHARACTER* 1  C 1 , C2 , C3 , C4 
INTEGER*2  IC(4) 

EQUIVALENCE  (C1,IC(1) ) , (C2,IC(2) ) , <C3,IC(3) ) , 

+  (C4 , IC (4) ) 

DATA  IC/1641B, 16#5B, 16#32, 16#4A/ 

WRITE <*,1)  C1,C2,C3,C4 
1  FORMAT (IX, 4A 1 ) 

RETURN 

•END 

SUBROUTINE  CUP(N,M> 


CHARACTERS  Cl  ,C2,C5,CS,LC  (5) 

CHARACTER*5  CBUFF 
INTEQER*2  IC(4) 

EQUIVALENCE  (C1,IC(1>  > , (C2,IC(2> > , <C5,IC<3> ) , 

+  <C8,  IC(4)  >,  (CBUFF, LC(1)  ) 

DATA  IC/16#1B, 16#5B, 16#3B, 16#66/ 

L3 l 0000+ 1 00*N+M 
WRITE (CBUFF, 2) L 
FORMAT (15) 

WR I TE ( * , 1 )  Cl ,C2,LC (2) ,LC(3> ,C5,LC(4) ,LC(5) ,C8 
FORMAT ( IX, SA1 ,\> 

RETURN 

END 


APPENDIX  D 

PLOTTING  PROGRAM  LISTING  FOR  MONITOR 


♦STORAGE : 2 
♦DEBUG 
♦NOLI ST 

C 

PROGRAM  MONITOR 
C 

CHARACTER*40  TITLE 
CHARACTER*35  LEGEND 
CHARACTER*25  NAMEX ,NAMEY 
REAL  X (245) ,Y(245) 

REAL  0(245) ,P(245) ,R(245) ,S<245) ,T(245) ,U(245) 
INTEGER*2  IC 
DATA  IC/O/ 

C 

C  USE  THESE  FOR  MULTIPLE  ARRAY  TRACKING 

C  TITLE  -MULTIPLE  ARRAY  ADAPTIVE  MANEUVERING  RUN' 

TITLE  -'MULTIPLE  ARRAY  ADAPTIVE  STRAIGHT  RUN' 

C 

C  USE  THESE  FOR  SINGLE  ARRAY  TRACKING 

C  TITLE  -SINGLE  ARRAY  ADAPTIVE  MANEUVERING  RUN ' 

C  TITLE  -'SINGLE  ARRAY  ADAPTIVE  STRAIGHT  RUN' 

OPEN (5, FI LE- ' XKK. DAT ' , STATUS- ' OLD ' ) 

DO  32  LENG  -  1  ,  241 

READ (5,*, END-33)  O (LENG) ,P (LENG) , R ( LENG ) , S (LENG) , 
+•  T (LENG) ,U (LENG) 

32  CONTINUE 

33  CONTINUE 

LENG  -  LENG  -  1 
CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEX  — ' XCK/K3  (FT)  ' 

NAMEY  -YCK/K3  (FT) ' 

LEGEND  -'FILTERED  ESTIMATE  OF  TRAJECTORY' 

CALL  DRAWER (TITLE, NAME  X , NAMEY , LEGEND , P , S , LENG ) 

OPEN ( 5 , F I LE- ' PKK . DAT ' , STATUS- ' OLD ' ) 

DO  34  LENG  »  1  ,  241 

READ (5,*, END-35)  O(LENG) , P (LENG) , R (LENG) , S (LENG) , 

+  T (LENG) ,U (LENG) 

34  CONTINUE 

35  CONTINUE 

LENG  -  LENG  -  1 
CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEX  -'TIME  SLOTS' 

NAMEY  -  PCK/K3 (1,1)' 

LEGEND  -'FILTERED  ERROR  COVARIANCE  P(K/K>  ' 

CALL  DRAWER ( T I TLE , NAME X , NAMEY , LEGEND , 0 , P , LENG  > 


NAMEY  -'PCK/K3 (2,2) ' 

CALL  DRAWER < T I TLE , NAMEX , NAMEY , LEGEND , 0 , R , LENG ) 
NAMEY  »'P CK/fO  <3, 3)  ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , 0 , S , LENG ) 
NAMEY  -'PCK/K3  <4, 4)  ' 

CALL  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , Q , T , LENG > 
NAMEY  * ' PCK/K] (5,5) ' 

CALL  DRAWER  <  T I TLE , NAMEX , NAMEY , LEGEND , 0 , U , LENG  > 
0PEN(5,FILE=' XKERR.DAT' , STATUS- 'OLD ' ) 

DO  38  LENG  -  1  ,  241 

READ (5,*, END-39)  0 (LENG) , P (LENG) ,R(LENG> , S (LENG) 

CONTINUE 

CONTINUE 

LENG  -  LENG  -  1. 

CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEY  = ' X  ERROR  (FT) ' 

LEGEND  -'ERROR  IN  FILTERED  ESTIMATE' 

CALL  DRAWER  < T I TLE , NAMEX , NAMEY , LEGEND , 0 , P , LENG ) 
NAMEY  -'Y  ERROR  (FT) ' 

CALL  DRAWER ( T I TLE , N AME  X , NAMEY , LEGEND , 0 , R , LENG ) 
NAMEY  -'Z  ERROR  (FT) ' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND ,  0 ,  S , LENG , 
OPEN ( 5 , F I LE- ' X  KN . D AT ' , STATUS- ' OLD ' ) 

DO  40  LENG  »  1  ,  241 

READ ( 5 ,  * , END— 4 1 )  0 (LENG) ,P(LENG> , R ( LENG ) , S (LENG) 
+  T (LENG) ,U (LENG) 

CONTINUE 

CONTINUE 

LENG  -  LENG  -  1 

CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEX  — ' XCK/N3  (FT) ' 

NAMEY  — ' YEK/N3  (FT)' 

LEGEND  -'SMOOTHED  ESTIMATE  OF  TRAJECTORY' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND , P , S , LENG ) 
OPEN ( 5  ,  F I LE- ' PKN . DAT ' , STATUS- ' OLD ' ) 

DO  42  LENG  -  1  ,  241 

READ ( 5, *, END-43)  O(LENG) ,P (LENG) , R ( LENG ) , S (LENG) 
+  T (LENG) ,U (LENG) 

CONTINUE 

CONTINUE 

LENG  -  LENG  -  1 

CLOSE ( 5 , STATUS- ' KEEP ' ) 

NAMEX  -'TIME  SLOTS' 

NAMEY  — 'PCK/N3 (1,1) ' 

LEGEND  -'SMOOTHED  ERROR  COVARIANCE  P(K/N)' 

CALL  DRAWER  ( T I  TLE ,  NAME  X  ,  NAMEY  ,  LEGEND ,  0 ,  P ,  LENG ) 
NAMEY  = ' PCK/N3 (2,2) ' 

CALL  DRAWER (TITLE, NAME X , NAMEY , LEGEND , O , R , LENG ) 
NAMEY  - 'PCK/NI (3,3)  ' 

CALL  DRAWER (TITLE, NAME  X , NAMEY , LEGEND , O , S , LENG ) 
NAMEY  »  PCK/N3 (4,4)  ' 


n  n  n  n  n 


CALL  DRAWER ( T I TLE , NAME X , NAMEY , LEGEND ,0,T, LENG ) 

NAMEY  — ' PCK/N3 (5,5) ' 

CALL  DRAWER (TITLE, NAME  X , NAMEY , LEGEND , 0 , U , LENG ) 

OPEN ( 5 , F I LE- ' XSERR . DAT  * , STATUS- ' OLD ' ) 

DO  44  LENG  »  1  ,  241 

READ (5,*, END-45)  O(LENG) , P (LENG) ,R (LENG) , S ( LENG ) 

44  CONTINUE 

45  CONTINUE 
LENG  -  LENG  -  1 
CLOSE (5 , STATUS- ' KEEP ' ) 

NAMEY  -'X  ERROR  (FT) * 

LEGEND  -'ERROR  IN  SMOOTHED  ESTIMATE' 

.  CALL  DRAWER (TITLE, NAMEX, NAMEY, LEGEND, 0,P, LENG) 

NAMEY  -'Y  ERROR  (FT) ' 

CALL  DRAWER ( T I f LE , NAME  X , NAMEY , LEGEND , 0 , R , LENG ) 

NAMEY  -'Z  ERROR  (FT)' 

CALL  DRAWER ( T I TLE , NAME  X , NAMEY , LEGEND , 0 , S , LENG ) 

STOP 
END 

SUBROUT I NE  DRAWER ( T I TLE , NAMEX , NAMEY , LEGEND , X , Y , LENG ) 
CHARACTER*40  TITLE 
CHARACTER*35  LEGEND 
CHARACTER*25  NAMEX, NAMEY 
REAL  X (245) , Y (245) 

INTEGER*2  IC 
DATA  IC/O/ 

CALL  ED 
CALL  CUP (1,0) 

CALL  PLOTS (0,99,99) 

CALL  SYMBOL (0.5,5. 15 , . 20 , TITLE , 0. O , 40) 

CALL  SYMBOL <1.04, 4. 75,. 175, LEGEND, O. 0,35) 

USE  THIS  FOR  NOISELESS  TRACKING 

CALL  SYMBOL <5. 38, 4. 75,. 175, 'WITHOUT  NOISE ' ,0. 0 , 13) 


USE  THIS 

FOR  NOISY  TRACKING 

CALL 

SYMBOL (5. 38, 4. 75,. 175, ' 

WITH  NOISE' ,0.0, 13) 

CALL 

PLOT <1.00, 1.00, -3) 

CALL 

SCALE ( X , 6 . 00 , LENG , 1 ) 

CALL 

SCALE ( Y , 3 . 00 , LENG , 1 ) 

CALL 

STAX  IS (. ISO, .20,. 15, . 1 12, 

-1) 

CALL 

AXIS (0. ,0. , NAMEX, -13, 6. 00 

,00. , X (LENG+1 ) 

+ 

X (LENG+2) ) 

CALL 

STAX  IS ( . 15,. 20,. 111,. 112, 

2) 

CALL 

AX  IS <0.  ,0.  , NAMEY, 13,3. 00, 

90. , Y  < LENG+ 1 ) , 

■¥ 

Y (LENG+2) ) 

CALL 

LINE (X,Y, LENG, 1,0,3) 

CALL 

PLOTXO. 0,0. 0,999) 

RETURN 

SUBROUTINE  ED 
CHARACTER* 1  C 1 , C2 , C3 , C4 
INTEGER*2  IC<4> 

EQUIVALENCE  (C1,IC<1> ) , (C2,IC<2> ) , <C3,IC<3> ) , 

+  <C4 , IC (4) ) 

DATA  IC/16#1B,16#5B,16#32,16#4A/ 

WR I TE ( * , 1 )  C1,C2,C3,C4 
FORMAT (1X,4A1) 

RETURN 

END 

SUBROUTINE  CUP(N,M> 

CHARACTER* 1  Cl ,C2,C5,C8,LC (5) 

CHARACTER*5  CBUFF 
INTESER*2  IC (4) 

EQUIVALENCE  (C1,IC(1>  > , (C2,IC(2>  > , (C3,IC(3> )  , 

+  <CS, IC<4) ), (CBUFF, LC<1) ) 

DATA  I C/ 1 6# 1 B , 1 6#5B , 1 6#3B , 1 6#66/ 

L»10000+100*N+M 

WRITE (CBUFF ,2) L 

FORMAT (IS) 

WRITE (*, 1 )  C1,C2,LC(2) ,LC(3> ,C5,LC(4) ,LC(5) ,C8 
FORMAT ( IX, SA1 ,\) 

RETURN 

END 


APPENDIX  E 
BATCH  FILES 

LISTINS  OF  AUTOEXEC.BAT  FILE  ON  OPERATING  SYSTEM  DISK 

ECHO  OFF 

GRAPHICS 

TIMER/S 

COPY  A: RUN. BAT  Cs 
COPY  As KEDIT.EXE  C: /V 
COPY  As  PROFILE. KED  Cs /V 
Cs 
RUN 

LISTING  OF  RUN. BAT  FILE  ON  VIRTUAL  DISK(C) 

ECHO.  Insert  the  disk,  which  has  the  source  -file  of  the 
ECHO,  sequential  extended  Kalman  filter  and  Smoothing, 
ECHO,  into  drive  A 
PAUSE 

COPY  As  THESIS. FOR  Cs 
KEDIT  Cs THESIS. FOR 
COPY  Cs THESIS. FOR  As 
ERASE  CsKEDIT.EXE 
ERASE  Cs PROFILE. KED 

ECHO.  Insert  the  disk,  which  has  PROFQRT.EXE  and 
ECHO.  LINK.EXE,  into  drive  A,  and  the  disk,  which  has 
ECHO.  PROFORT. LIB  into  drive  B. 

PAUSE 

AsPROFORT  THESIS  /L  /E 

As  LINK  THESIS, , NULL, PROFORT 

ERASE  Cs THESIS. FOR 

ERASE  Cs THESIS. OBJ 

THESIS 

ECHO.  Insert  the  disk,  which  has  the  source  file  of  the 
ECHO,  sequential  extended  Kalman  filter  and  Smoothing, 
ECHO,  into  drive  A,  and  the  disk  labeled  "DATA"  into 
ECHO,  drive  B. 

PAUSE 

COPY  Cs  THESIS. EXE  A: 

COPY  C: *. DAT  Bs 
ERASE  C: *. * 

ECHO.  Insert  the  operating  system  disk  into  drive  A,  an' 
ECHO,  the  disk,  which  has  the  plotting  routine  source 
ECHO,  file  into  drive  B. 

PAUSE 

COPY  As  KEDIT. EXE  Cs 
COPY  As  PROFILE. KED  Cs 
COPY  Bs GRAPH. FOR  Cs 
KEDIT  Cs GRAPH. FOR 
COPY  Cs GRAPH. FOR  Bs 


ERASE  C: KEDIT.EXE 
ERASE  Cs PROFILE. KED 

ECHO.  Insert  the  disk,  which  has  FORl.EXE  and  PAS2.EXE 
ECHO,  into  drive  A,  and  the  disk,  which  has  PL0TB8.LIB 
ECHO,  into  drive  B. 

PAUSE 

A i FORI  GRAPH; 

A: PAS2 

ECHO.  Insert  the  disk,  which  has  FORTRAN. LIB,  MATH. LIB 
ECHO,  and  LINK. EXE  into  drive  A. 

PAUSE 

A:  LINK  GRAPH,  , NULL, B:  PLOTTSS+A-. FORTRAN+A:  MATH 
ECHO.  Insert  the  disk,  which  has  the  platting  source 
ECHO,  file  into  drive  A  and  the  data  disk  into  drive  B 
PAUSE 

COPY  C: GRAPH. EXE  As 
ERASE  C: GRAPH. FOR 
ERASE  C: GRAPH. OBJ 
COPY  B: *. DAT  C: 

GRAPH 
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